/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  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 NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Daniela Cabiddu                                                           *
*     http://www.imati.cnr.it/index.php/people/8-curricula/119-daniela-cabiddu  *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/polygon_maximum_inscribed_circle.h>
#include <cinolib/geometry/segment.h>
#include <cinolib/min_max_inf.h>

// Most of this is coming from here:
// http://www.boost.org/doc/libs/1_65_1/libs/polygon/doc/voronoi_diagram.htm
// http://www.boost.org/doc/libs/1_65_0/libs/polygon/example/voronoi_basic_tutorial.cpp
//
#ifdef CINOLIB_USES_BOOST
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry.hpp>
#include <boost/polygon/voronoi.hpp>
//
using boost::polygon::voronoi_builder;
using boost::polygon::voronoi_diagram;
using boost::polygon::voronoi_edge;
using boost::polygon::x;
using boost::polygon::y;
using boost::polygon::low;
using boost::polygon::high;
//
struct polygon_point
{
    int x;
    int y;
    polygon_point(int x, int y) : x(x), y(y) {}
};
//
struct polygon_segment
{
    polygon_point p0;
    polygon_point p1;
    polygon_segment(int x1, int y1, int x2, int y2) : p0(x1, y1), p1(x2, y2) {}
};
//
template<>
struct boost::polygon::geometry_concept<polygon_point>
{
    typedef boost::polygon::point_concept type;
};
//
template<>
struct boost::polygon::point_traits<polygon_point>
{
    typedef int coordinate_type;
    static inline coordinate_type get(const polygon_point & point, orientation_2d orient)
    {
        return (orient == HORIZONTAL) ? point.x : point.y;
    }
};
//
template<>
struct boost::polygon::geometry_concept<polygon_segment>
{
    typedef boost::polygon::segment_concept type;
};
//
template<>
struct boost::polygon::segment_traits<polygon_segment>
{
    typedef int coordinate_type;
    typedef polygon_point point_type;

    static inline point_type get(const polygon_segment& segment, direction_1d dir)
    {
        return dir.to_int() ? segment.p1 : segment.p0;
    }
};
//
typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint>  BoostPolygon;

#endif // CINOLIB_USES_BOOST

namespace cinolib
{

#ifdef CINOLIB_USES_BOOST

BoostPolygon make_boost_poly(const std::vector<vec2d> & poly)
{
    BoostPolygon boost_poly;
    for(vec2d p : poly) boost::geometry::append(boost_poly, BoostPoint(p.x(), p.y()));
    boost::geometry::correct(boost_poly);
    return boost_poly;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void polygon_maximum_inscribed_circle(const std::vector<vec2d> & poly,
                                            vec2d              & center,
                                            double             & radius)
{
    radius = 0.0;
    center = vec2d(0,0);

    vec2d min( inf_double,  inf_double);
    vec2d max(-inf_double, -inf_double);
    for(auto p : poly)
    {
        min = min.min(p);
        max = max.max(p);
    }

    double scale_factor;
    double diag = min.dist(max);

    if(diag >= 1.0) scale_factor = std::numeric_limits<int>::max() / diag;
    else            scale_factor = std::numeric_limits<int>::max() * diag;

    scale_factor *= 0.5; // dumb attempt to stay on the safe side and avoid overflows...

    // Boost implementation of Generazlied Voronoi uses integer coordinates.
    // In order to achieve maximum precision I am scaling the polygon as much
    // as I can in order to minimize loss of precision during integer roundoff...

    std::vector<polygon_segment> segments;
    for(uint i=0; i<poly.size(); ++i)
    {
        vec2d v0 = scale_factor * poly.at(i);
        vec2d v1 = scale_factor * poly.at((i+1)%poly.size());
        segments.push_back(polygon_segment(v0.x(), v0.y(), v1.x(), v1.y()));
    }

    voronoi_diagram<double> vd;
    construct_voronoi(segments.begin(), segments.end(), &vd);

    BoostPolygon boost_poly = make_boost_poly(poly);
    for(auto it=vd.vertices().begin(); it!=vd.vertices().end(); ++it)
    {
        const voronoi_diagram<double>::vertex_type &v = *it;
        const voronoi_diagram<double>::edge_type   *e = v.incident_edge();
        const voronoi_diagram<double>::cell_type   *c = e->cell();
        const polygon_segment                        &s = segments.at(c->source_index());

        // do not consider Voronoi vertices outside the polygon
        if (boost::geometry::within(BoostPoint(v.x()/scale_factor, v.y()/scale_factor), boost_poly))
        {
            // annoying wrap to vec3d (TODO: template cinolib::Segment to make it work in 2D too)
            vec3d beg(s.p0.x, s.p0.y, 0);
            vec3d end(s.p1.x, s.p1.y, 0);
            vec3d c3d(v.x(),  v.y(),  0);
            cinolib::Segment tmp(beg,end);
            double d = tmp.dist_to_point(c3d);

            if (d > radius)
            {
                radius = d;
                center = vec2d(c3d); // will automatically drop z
            }
        }
    }

    radius /= scale_factor;
    center /= scale_factor;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

CINO_INLINE
void polygon_maximum_inscribed_circle(const std::vector<vec3d> & poly,   // will drop z component
                                            vec3d              & center, // will have z=0
                                            double             & radius)
{
    std::vector<vec2d> poly_2d;
    for(auto p : poly) poly_2d.push_back(vec2d(p.x(), p.y()));

    vec2d center_2d;
    polygon_maximum_inscribed_circle(poly_2d, center_2d, radius);
    center = vec3d(center_2d.x(), center_2d.y(), 0);
}

#endif // CINOLIB_USES_BOOST

}


//
// Created by jianping on 17-9-11.
//

#include <iostream>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <fstream>
#include <Eigen/Geometry>

//image intrinsics
//reference image r,t
//source image r,t
//https://github.com/opencv/opencv/blob/master/samples/cpp/stereo_match.cpp

void saveXYZ(const char* filename, const cv::Mat& mat,float tx)
{
    const double max_z = 10000;
    FILE* fp = fopen(filename, "wt");
    for(int y = 0; y < mat.rows; y++)
    {
        for(int x = 0; x < mat.cols; x++)
        {
            cv::Vec3f point = mat.at<cv::Vec3f>(y, x);
            if(fabs(fabs(point[2]) - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
            fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
        }
    }
    fclose(fp);
}

void readParams(const std::string& intrinsicFile,
                Eigen::Matrix3d& K,
                Eigen::Matrix3d& C,
                Eigen::Vector3d& t)
{
    std::ifstream ifs(intrinsicFile.c_str(),std::ios_base::in);
    char line[256];
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&K(0,0),&K(0,1),&K(0,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&K(1,0),&K(1,1),&K(1,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&K(2,0),&K(2,1),&K(2,2));
    ifs.getline(line,256);//useless
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&C(0,0),&C(0,1),&C(0,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&C(1,0),&C(1,1),&C(1,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&C(2,0),&C(2,1),&C(2,2));
    ifs.getline(line,256);
    sscanf(line,"%lf %lf %lf",&t(0),&t(1),&t(2));
    std::cout<<K<<"\n"<<C<<"\n"<<t.transpose()<<"\n";

}

const int scale = 1;
int main()
{
    LOG(INFO)<<"matching"<<std::endl;
    cv::Mat image_ref_origin = cv::imread("rdimage.001.ppm");
    cv::Mat image_source_origin = cv::imread("rdimage.000.ppm");
    cv::Mat image_ref,image_source;

    cv::resize(image_ref_origin,image_ref,
               cv::Size(image_ref_origin.cols/scale,image_ref_origin.rows/scale),
               0,0,cv::INTER_CUBIC);
    cv::resize(image_source_origin,image_source,
               cv::Size(image_ref_origin.cols/scale,image_ref_origin.rows/scale),
               0,0,cv::INTER_CUBIC);

    Eigen::Matrix3d K0,C0,K1,C1,C;
    Eigen::Vector3d r0,r1,r;
    readParams("rdimage.001.ppm.camera",K0,C0,r0);
    readParams("rdimage.000.ppm.camera",K1,C1,r1);
    {
        K0(0,0)/=scale;
        K0(1,1)/=scale;
        K0(0,2)/=scale;
        K0(1,2)/=scale;
        K1(0,0)/=scale;
        K1(1,1)/=scale;
        K1(0,2)/=scale;
        K1(1,2)/=scale;
    }
    //C = C0.transpose()*C1;
    //r = C0.transpose()*(r1-r0);
    C = C1.transpose()*C0;// transform p from frame 0 to frame 1
    r = C1.transpose()*(r0-r1);
    Eigen::Quaterniond q(C);
    C = q.toRotationMatrix();
    cv::Mat Q;
    cv::Rect roi1, roi2;
    cv::Mat M0,M1,R01(3,3,CV_64FC1),t(3,1,CV_64FC1),R0__,R1__,P0__,P1__;
    cv::Mat D0(5,1,CV_64FC1),D1(5,1,CV_64FC1);
    D0.setTo(0);
    D1.setTo(0);
    cv::eigen2cv(K0,M0);
    cv::eigen2cv(K1,M1);
    cv::eigen2cv(C,R01);
    cv::eigen2cv(r,t);

    cv::Size size(image_ref.cols, image_ref.rows);
    cv::stereoRectify(M0,D0,M1,D1,
                      size,R01,t,
                      R0__,R1__,
                      P0__,P1__,
                      Q,cv::CALIB_FIX_INTRINSIC,-1,
                      size, &roi1, &roi2
    );

    cv::Mat map11, map12, map21, map22;
    cv::initUndistortRectifyMap(M0, D0, R0__, P0__, size, CV_16SC2, map11, map12);
    cv::initUndistortRectifyMap(M1, D1, R1__, P1__, size, CV_16SC2, map21, map22);
    //std::cout<<P0__<<"\n"<<R1__<<std::endl;
    cv::Mat img1r, img2r;
    cv::remap(image_ref, img1r, map11, map12, cv::INTER_LINEAR);
    cv::remap(image_source, img2r, map21, map22, cv::INTER_LINEAR);

    image_ref = img1r;
    image_source = img2r;

//    cv::resize(img1r,image_ref,cv::Size(size.width/3,size.height/3));
//    cv::resize(img2r,image_source,cv::Size(size.width/3,size.height/3));

    //cv::waitKey(0);

    cv::StereoSGBM sgbm(-512,1024,
                        8,8*3*3*3,32*3*3*3,2,
                        16,5,100,
                        2,false
    );

    cv::Mat disp,disp8,dispnorm(image_ref.rows,image_ref.cols,CV_64FC1);
    sgbm(image_ref,image_source,disp);

//    cv::normalize(disp,dispnorm,255,0,cv::NORM_MINMAX);
//    dispnorm.convertTo(disp8, CV_8U);
//    cv::namedWindow("re",cv::WINDOW_NORMAL);
//    cv::imshow("re",disp8);

    cv::namedWindow("ref",cv::WINDOW_NORMAL);
    cv::namedWindow("source",cv::WINDOW_NORMAL);

    for (int i = 0; i < image_ref.rows; i += image_ref.rows/20 ) {
        cv::line(image_ref,cv::Point(0,i),cv::Point(image_ref.cols,i),cv::Scalar(0,255,0),1);
        cv::line(image_source,cv::Point(0,i),cv::Point(image_ref.cols,i),cv::Scalar(0,255,0),1);

    }

    cv::imshow("ref",image_ref);
    cv::imshow("source",image_source);

    cv::Mat xyz,xyz8;
    reprojectImageTo3D(disp, xyz, Q, false);

    saveXYZ("./pts.xyz", xyz, r(0));
    cv::waitKey(0);
    return 0;
}


//    Copyright 2021 Jij Inc.

//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at

//        http://www.apache.org/licenses/LICENSE-2.0

//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.

#ifndef OPENJIJ_GRAPH_DENSE_HPP__
#define OPENJIJ_GRAPH_DENSE_HPP__

#include <map>
#include <vector>
#include <cassert>
#include <cstddef>
#include <type_traits>
#include <algorithm>
#include <exception>

#include <utility/disable_eigen_warning.hpp>
#include <Eigen/Dense>

#include <graph/json/parse.hpp>
#include <graph/graph.hpp>

namespace openjij {
    namespace graph {

        /**
         * @brief two-body all-to-all interactions
         * The Hamiltonian is like
         * \f[
         * H = \sum_{i<j}J_{ij} \sigma_i \sigma_j + \sum_{i}h_{i} \sigma_i
         * \f]
         *
         * @tparam FloatType float type of Sparse class (double or float)
         */
        template<typename FloatType>
            class Dense : public Graph{
                static_assert(std::is_floating_point<FloatType>::value, "FloatType must be floating-point type.");
                public:

                    /**
                     * @brief interaction type (Eigen)
                     * The stored matrix has the following triangular form:
                     *
                     * \f[
                     * \begin{pmatrix}
                     * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
                     * 0 & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
                     * \vdots & \vdots & \vdots & \vdots & \vdots \\
                     * 0 & 0 & \cdots & J_{N-1,N-1} & h_{N-1}\\
                     * 0 & 0 & \cdots & 0 & 1 \\
                     * \end{pmatrix}
                     * \f]
                     */
                    using Interactions = Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

                    /**
                     * @brief float type
                     */
                    using value_type = FloatType;

                private:

                    /**
                     * @brief interactions
                     */
                    Interactions _J;

                public:

                    /**
                     * @brief Dense constructor
                     *
                     * @param num_spins the number of spins
                     */
                    explicit Dense(std::size_t num_spins)
                        : Graph(num_spins), _J(Interactions::Zero(num_spins+1, num_spins+1)){
                            _J(num_spins, num_spins) = 1;
                        }

                    /**
                     * @brief Dense constructor (from nlohmann::json)
                     *
                     * @param j JSON object this object must be a serialized object of dense BQM.
                     */
                    Dense(const json& j) : Dense(static_cast<size_t>(j["num_variables"])){
                        //define bqm with ising variables
                        auto bqm = json_parse<FloatType, cimod::Dense>(j);
                        //interactions
                        //for(auto&& elem : bqm.get_quadratic()){
                        //    const auto& key = elem.first;
                        //    const auto& val = elem.second;
                        //    J(key.first, key.second) += val;
                        //}
                        //local field
                        //for(auto&& elem : bqm.get_linear()){
                        //    const auto& key = elem.first;
                        //    const auto& val = elem.second;
                        //    h(key) += val;
                        //}
                        // the above insertion is simplified as
                        this->_J = bqm.interaction_matrix();
                    }

                    /**
                     * @brief set interaction matrix from Eigen Matrix.
                     *
                     * @param interaction Eigen matrix
                     */
                    void set_interaction_matrix(const Interactions& interaction){
                        if(interaction.rows() != interaction.cols()){
                            std::runtime_error("interaction.rows() != interaction.cols()");
                        }

                        if((size_t)interaction.rows() != get_num_spins() + 1){
                            throw std::runtime_error("invalid matrix size.");
                        }

                        //check if diagonal elements are zero
                        for(size_t i=0; i<(size_t)(interaction.rows()-1); i++){
                            if(interaction(i,i) != 0){
                                throw std::runtime_error("The diagonal elements of interaction matrix must be zero.");
                            }
                        }

                        if(interaction(interaction.rows()-1,interaction.rows()-1) != 1){
                            throw std::runtime_error("The right bottom element of interaction matrix must be unity.");
                        }

                        _J = interaction.template selfadjointView<Eigen::Upper>();
                    }

                    /**
                     * @brief Dense copy constructor
                     */
                    Dense(const Dense<FloatType>&) = default;

                    /**
                     * @brief Dense move constructor
                     */
                    Dense(Dense<FloatType>&&) = default;

                    /**
                     * @brief calculate total energy
                     *
                     * @param spins
                     * @deprecated use energy(spins)
                     *
                     * @return corresponding energy
                     */
                    FloatType calc_energy(const Spins& spins) const{
                        return this->energy(spins);
                    }

                    FloatType calc_energy(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>& spins) const{
                        return this->energy(spins);
                    }

                    /**
                     * @brief calculate total energy
                     *
                     * @param spins
                     *
                     * @return corresponding energy
                     */
                    FloatType energy(const Spins& spins) const{
                        if(spins.size() != this->get_num_spins()){
                            throw std::out_of_range("Out of range in energy in Dense graph.");
                        }

                        using Vec = Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>;
                        Vec s(get_num_spins()+1);
                        for(size_t i=0; i<spins.size(); i++){
                            s(i) = spins[i];
                        }
                        s(get_num_spins()) = 1;

                        // the energy must be consistent with BinaryQuadraticModel.
                        return (s.transpose()*(_J.template triangularView<Eigen::Upper>()*s))(0,0)-1;
                    }

                    FloatType energy(const Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>& spins) const{
                        graph::Spins temp_spins(get_num_spins());
                        for(size_t i=0; i<temp_spins.size(); i++){
                            temp_spins[i] = spins(i);
                        }
                        return energy(temp_spins);

                    }

                    /**
                     * @brief access J_{ij}
                     *
                     * @param i Index i
                     * @param j Index j
                     *
                     * @return J_{ij}
                     */
                    FloatType& J(Index i, Index j){
                        assert(i < get_num_spins());
                        assert(j < get_num_spins());

                        if(i != j)
                            return _J(std::min(i, j), std::max(i, j));
                        else
                            return _J(std::min(i, j), get_num_spins());
                    }

                    /**
                     * @brief access J_{ij}
                     *
                     * @param i Index i
                     * @param j Index j
                     *
                     * @return J_{ij}
                     */
                    const FloatType& J(Index i, Index j) const{
                        assert(i < get_num_spins());
                        assert(j < get_num_spins());

                        if(i != j)
                            return _J(std::min(i, j), std::max(i, j));
                        else
                            return _J(std::min(i, j), get_num_spins());
                    }

                    /**
                     * @brief access h_{i} (local field)
                     *
                     * @param i Index i
                     *
                     * @return h_{i}
                     */
                    FloatType& h(Index i){
                        assert(i < get_num_spins());
                        return J(i, i);
                    }

                    /**
                     * @brief access h_{i} (local field)
                     *
                     * @param i Index i
                     *
                     * @return h_{i}
                     */
                    const FloatType& h(Index i) const{
                        assert(i < get_num_spins());
                        return J(i, i);
                    }

                    /**
                     * @brief get interactions (Eigen Matrix)
                     *
                     * The returned matrix has the following symmetric form:
                     *
                     * \f[
                     * \begin{pmatrix}
                     * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
                     * J_{0,1} & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
                     * \vdots & \vdots & \vdots & \vdots & \vdots \\
                     * J_{0,N-1} & J_{N-1,1} & \cdots & J_{N-1,N-1} & h_{N-1}\\
                     * h_{0} & h_{1} & \cdots & h_{N-1} & 1 \\
                     * \end{pmatrix}
                     * \f]
                     *
                     * @return Eigen Matrix
                     */
                    const Interactions get_interactions() const{
                        return this->_J.template selfadjointView<Eigen::Upper>();
                    }

            };
    } // namespace graph
} // namespace openjij

#endif


// Copyright (C) 2016 Kasper Kristensen
// License: GPL-2

#ifndef TINY_AD_H
#define TINY_AD_H

/* Standalone ? */
#ifndef R_RCONFIG_H
#include <cmath>
#include <iostream>
#define CSKIP(x) x
#endif

/* Select the vector class to use (Default: tiny_vec) */
#if defined(TINY_AD_USE_STD_VALARRAY)
#include "tiny_valarray.hpp"
#define TINY_VECTOR(type,size) tiny_vector<type, size>
#elif defined(TINY_AD_USE_EIGEN_VEC)
#include <Eigen/Dense>
#define TINY_VECTOR(type,size) Eigen::Array<type, size, 1>
#else
#include "tiny_vec.hpp"
#define TINY_VECTOR(type,size) tiny_vec<type, size>
#endif

namespace tiny_ad {
  template<class Type, class Vector>
  struct ad {
    Type value;
    Vector deriv;
    ad(){}
    ad(Type v, Vector d){value = v; deriv = d;}
    ad(double v)        {value = v; deriv.setZero();}
    ad operator+ (const ad &other) const{
      return ad(value + other.value,
		deriv + other.deriv);
    }
    ad operator+ () const{
      return *this;
    }
    ad operator- (const ad &other) const{
      return ad(value - other.value,
		deriv - other.deriv);
    }
    ad operator- () const{
      return ad(-value, -deriv);
    }
    ad operator* (const ad &other) const{
      return ad(value * other.value,
		value * other.deriv +
		deriv * other.value);
    }
    ad operator/ (const ad &other) const{
      Type res = value / other.value;
      return ad(res,
		(deriv - res * other.deriv) /
		other.value );
    }
    /* Comparison operators */
#define COMPARISON_OPERATOR(OP)			\
    template<class other>			\
    bool operator OP (const other &x) const{	\
      return (value OP x);			\
    }
    COMPARISON_OPERATOR(<)
    COMPARISON_OPERATOR(>)
    COMPARISON_OPERATOR(<=)
    COMPARISON_OPERATOR(>=)
    COMPARISON_OPERATOR(==)
    COMPARISON_OPERATOR(!=)
#undef COMPARISON_OPERATOR
    /* Combine ad with other types (constants) */
    ad operator+ (const double &x) const{
      return ad(value + x, deriv);
    }
    ad operator- (const double &x) const{
      return ad(value - x, deriv);
    }
    ad operator* (const double &x) const{
      return ad(value * x, deriv * x);
    }
    ad operator/ (const double &x) const{
      return ad(value / x, deriv / x);
    }
    /* Note: 'this' and 'other' may point to the same object */
    ad& operator+=(const ad &other){
      value += other.value;
      deriv += other.deriv;
      return *this;
    }
    ad& operator-=(const ad &other){
      value -= other.value;
      deriv -= other.deriv;
      return *this;
    }
    ad& operator*=(const ad &other){
      if (this != &other) {
	deriv *= other.value;
	deriv += other.deriv * value;
	value *= other.value;
      } else {
	deriv *= value * 2.;
	value *= value;
      }
      return *this;
    }
    ad& operator/=(const ad &other){
      value /= other.value;
      deriv -= other.deriv * value;
      deriv /= other.value;
      return *this;
    }
  };
  /* Binary operators where a constant is first argument */
  template<class T, class V>
  ad<T, V> operator+ (const double &x, const ad<T, V> &y) {
    return y + x;
  }
  template<class T, class V>
  ad<T, V> operator- (const double &x, const ad<T, V> &y) {
    return -(y - x);
  }
  template<class T, class V>
  ad<T, V> operator* (const double &x, const ad<T, V> &y) {
    return y * x;
  }
  template<class T, class V>
  ad<T, V> operator/ (const double &x, const ad<T, V> &y) {
    T value = x / y.value;
    return ad<T, V>(value, T(-value / y.value) * y.deriv);
  }
  /* Unary operators with trivial derivatives */
#define UNARY_MATH_ZERO_DERIV(F)		\
  template<class T, class V>			\
  double F (const ad<T, V> &x){			\
    return F(x.value);				\
  }
  using ::floor; using ::ceil;
  using ::trunc; using ::round;
  UNARY_MATH_ZERO_DERIV(floor)
  UNARY_MATH_ZERO_DERIV(ceil)
  UNARY_MATH_ZERO_DERIV(trunc)
  UNARY_MATH_ZERO_DERIV(round)
  template<class T>
  double sign(const T &x){return (x > 0) - (x < 0);}
  bool isfinite(const double &x)CSKIP( {return std::isfinite(x);} )
  template<class T, class V>
  bool isfinite(const ad<T, V> &x){return isfinite(x.value);}
#undef UNARY_MATH_ZERO_DERIV
  /* Unary operators with non-trivial derivatives */
#define UNARY_MATH_DERIVATIVE(F,DF)		\
  template<class T, class V>			\
  ad<T, V> F (const ad<T, V> &x){		\
    return ad<T, V>(F (x.value),		\
		    T(DF(x.value)) * x.deriv);	\
  }
  using ::exp;  using ::log;
  using ::sin;  using ::cos;  using ::tan;
  using ::sinh; using ::cosh; using ::tanh;
  using ::sqrt; using ::fabs;
  template<class T> T D_tan(const T &x) {
    T y = cos(x); return 1. / (y * y);
  }
  template<class T> T D_tanh(const T &x) {
    T y = cosh(x); return 1. / (y * y);
  }
  UNARY_MATH_DERIVATIVE(exp, exp)
  UNARY_MATH_DERIVATIVE(log, 1.0/)
  UNARY_MATH_DERIVATIVE(sin, cos)
  UNARY_MATH_DERIVATIVE(cos, -sin)
  UNARY_MATH_DERIVATIVE(tan, D_tan)
  UNARY_MATH_DERIVATIVE(sinh, cosh)
  UNARY_MATH_DERIVATIVE(cosh, sinh)
  UNARY_MATH_DERIVATIVE(tanh, D_tanh)
  UNARY_MATH_DERIVATIVE(sqrt, 0.5/sqrt)
  UNARY_MATH_DERIVATIVE(fabs, sign)
  using ::expm1; using ::log1p;
  UNARY_MATH_DERIVATIVE(expm1, exp)
  template<class T> T D_log1p(const T &x) {return 1. / (x + 1.);}
  UNARY_MATH_DERIVATIVE(log1p, D_log1p)
  /* asin, acos, atan */
  using ::asin; using ::acos; using ::atan;
  template<class T> T D_asin(const T &x) {
    return 1. / sqrt(1. - x * x);
  }
  template<class T> T D_acos(const T &x) {
    return -1. / sqrt(1. - x * x);
  }
  template<class T> T D_atan(const T &x) {
    return 1. / (1. + x * x);
  }
  UNARY_MATH_DERIVATIVE(asin, D_asin)
  UNARY_MATH_DERIVATIVE(acos, D_acos)
  UNARY_MATH_DERIVATIVE(atan, D_atan)
#undef UNARY_MATH_DERIVATIVE
  /* A few more ... */
  template<class T, class V>
  ad<T, V> pow (const ad<T, V> &x, const ad<T, V> &y){
    return exp(y * log(x));
  }
  using ::pow;
  template<class T, class V>
  ad<T, V> pow (const ad<T, V> &x, const double &y){
    return ad<T, V> (pow(x.value, y), // Note: x.value could be 0
		     T( y * pow(x.value, y - 1.) ) * x.deriv);
  }
  /* Comparison operators where a constant is first argument */
#define COMPARISON_OPERATOR_FLIP(OP1, OP2)			\
  template<class T, class V>					\
  bool operator OP1 (const double &x, const ad<T, V> &y) {	\
    return y OP2 x;						\
  }
  COMPARISON_OPERATOR_FLIP(<,>)
  COMPARISON_OPERATOR_FLIP(<=,>=)
  COMPARISON_OPERATOR_FLIP(>,<)
  COMPARISON_OPERATOR_FLIP(>=,<=)
  COMPARISON_OPERATOR_FLIP(==,==)
  COMPARISON_OPERATOR_FLIP(!=,!=)
#undef COMPARISON_OPERATOR_FLIP
  /* Utility: Return the value of a tiny_ad type */
  double asDouble(double x) CSKIP( {return x;} )
  template<class T, class V>
  double asDouble (const ad<T, V> &x){
    return asDouble(x.value);
  }
  /* Utility: Return the max absolute value of all members of a
     tiny_ad type */
  double max_fabs(double x) CSKIP( {return fabs(x);} )
  template<class T, class V>
  double max_fabs (const ad<T, V> &x){
    double ans = max_fabs(x.value);
    for(int i=0; i<x.deriv.size(); i++) {
      double tmp = max_fabs(x.deriv[i]);
      ans = (tmp > ans ? tmp : ans);
    }
    return ans;
  }
  /* R-specific derivatives (rely on Rmath)*/
#ifdef R_RCONFIG_H
  extern "C" {
    /* See 'R-API: entry points to C-code' (Writing R-extensions) */
    double	Rf_lgammafn(double);
    double	Rf_psigamma(double, double);
  }
  template<int deriv>
  double lgamma(const double &x) {
    return Rf_psigamma(x, deriv-1);
  }
  template<>
  double lgamma<0>(const double &x) CSKIP( {return Rf_lgammafn(x);} )
  double lgamma(const double &x) CSKIP( {return lgamma<0>(x);} )
  template<int deriv, class T, class V>
  ad<T, V> lgamma (const ad<T, V> &x){
    return ad<T, V> (lgamma< deriv >(x.value),
		     T(lgamma< deriv + 1 >(x.value)) * x.deriv);
  }
  template<class T, class V>
  ad<T, V> lgamma (const ad<T, V> &x){
    return lgamma<0>(x);
  }
#endif
  /* Print method */
  template<class T, class V>
  std::ostream &operator<<(std::ostream &os, const ad<T, V> &x) {
    os << "{";
    os << " value=" << x.value;
    os << " deriv=" << x.deriv;
    os << "}";
    return os;
  }

  /* Interface to higher order derivatives. Example:

     typedef tiny_ad::variable<3, 2> Float; // Track 3rd order derivs wrt. 2 parameters
     Float a (1.23, 0);                     // Let a = 1.23 have parameter index 0
     Float b (2.34, 1);                     // Let b = 2.34 have parameter index 1
     Float y = sin(a + b);                  // Run the algorithm
     y.getDeriv();                          // Get all 3rd order derivatives
  */
#define VARIABLE(order, nvar, scalartype) variable<order, nvar, scalartype>
  template<int order, int nvar, class Double=double>
  struct variable : ad< VARIABLE(order-1, nvar, Double),
			TINY_VECTOR( VARIABLE(order-1, nvar, Double) , nvar) > {
    typedef ad< VARIABLE(order-1, nvar, Double),
		TINY_VECTOR(VARIABLE(order-1, nvar, Double), nvar) > Base;
    typedef variable<order-1, nvar, Double> Type;
    static const int result_size = nvar * Type::result_size;
#define ___COMMON_CTORS___                      \
    variable() {}                               \
    variable(Base x) : Base(x) {}               \
    variable(double x) : Base(x) {}             \
    variable(double x, int id) : Base(x) {      \
      setid(id);                                \
    }                                           \
    template<int T1, int T2, class T3>          \
    variable(variable<T1,T2,T3> x) {            \
      Base::value = x; Base::deriv.setZero();   \
    }                                           \
    template<class T1, class T2>                \
    variable(ad<T1,T2> x) {                     \
      Base::value = x; Base::deriv.setZero();   \
    }                                           \
    template<int T1, int T2, class T3>          \
    variable(variable<T1,T2,T3> x, int id) {    \
      Base::value = x; Base::deriv.setZero();   \
      setid(id);                                \
    }                                           \
    template<class T1, class T2>                \
    variable(ad<T1,T2> x, int id) {             \
      Base::value = x; Base::deriv.setZero();   \
      setid(id);                                \
    }
    ___COMMON_CTORS___
    void setid(int i0, int count = 0){
      this->value.setid(i0, count);
      this->deriv[i0].setid(i0, count + 1);
    }
    TINY_VECTOR(Double, result_size) getDeriv(){
      TINY_VECTOR(Double, result_size) ans;
      int stride = result_size / nvar;
      for(int i=0; i<nvar; i++)
	ans.segment(i * stride, stride) = this->deriv[i].getDeriv();
      return ans;
    }
  };
#undef VARIABLE
  template<int nvar, class Double>
  struct variable<1, nvar, Double> : ad<Double, TINY_VECTOR(Double,nvar) >{
    typedef ad<Double, TINY_VECTOR(Double,nvar) > Base;
    static const int result_size = nvar;
    ___COMMON_CTORS___
    void setid(int i0, int count = 0){
      if(count == 0)
	this->deriv[i0] = 1.0;
      if(count == 1)
	this->value = 1.0;
    }
    TINY_VECTOR(Double, nvar) getDeriv(){
      return this->deriv;
    }
  };
#undef ___COMMON_CTORS___
#undef TINY_VECTOR
} // End namespace tiny_ad

#endif


/**
 * @file BooleanOperation.hpp
 * @author bwu
 * @brief Boolean operation for geometries
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_BOOLEANOPERATION_HPP
#define GENERIC_GEOMETRY_BOOLEANOPERATION_HPP
#include "BoostPolygonRegister.hpp"
#include "GeometryTraits.hpp"
#include <boost/polygon/polygon_set_traits.hpp>
#include <boost/polygon/polygon_set_data.hpp>
namespace generic  {
namespace geometry {
///@brief boolean operations of geometry
namespace boolean  {

template <typename num_type>
using PolygonSet2D = boost::polygon::polygon_set_data<num_type>;

/**
 * @brief Boolean OR (polygon set union) operation of two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the union result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Unite(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet += g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean OR (polygon set union) operation of a collection of geometries
 * @param[in] begin iterator to the beginning of the geometry collection
 * @param[in] end iterator to the ending of the geometry collection
 * @param[out] results stl container with Polygon2D that hold the union result of the collection of geometries
 */
template <typename iterator,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<
          typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
inline void Unite(iterator begin, iterator end,
                  container<polygon_type<typename std::iterator_traits<iterator>::value_type>,
                  allocator<polygon_type<typename std::iterator_traits<iterator>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet;
    for(auto iter = begin; iter != end; ++iter)
        polygonSet += *iter;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean AND (polygon set intersection) operation of a two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the intersection result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Intersect(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet &= g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean AND (polygon set intersection) operation of two geometry sets
 * @param[in] begin1 iterator to the beginning of the geometry collection one
 * @param[in] end1 iterator to the ending of the geometry collection one
 * @param[in] begin2 iterator to the beginning of the geometry collection two
 * @param[in] end2 iterator to the ending of the geometry collection two
 * @param[out] results stl container with Polygon2D that hold the intersection result of two geometry sets
 */
template <typename iterator1, typename iterator2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator1>::value_type>::value &&
                                  traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator2>::value_type>::value, bool>::type = true>
inline void Intersect(iterator1 begin1, iterator1 end1, iterator2 begin2, iterator2 end2,
                      container<polygon_type<typename std::iterator_traits<iterator1>::value_type>,
                      allocator<polygon_type<typename std::iterator_traits<iterator1>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator1>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet1, polygonSet2;
    for(auto iter1 = begin1; iter1 != end1; ++iter1)
        polygonSet1 += *iter1;
    for(auto iter2 = begin2; iter2 != end2; ++iter2)
        polygonSet2 += *iter2;
    polygonSet1 *= polygonSet2;
    results.clear();
    polygonSet1.get(results);
}

/**
 * @brief Boolean SUBTRACT operation (polygon set difference) of two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the difference result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Subtract(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet -= g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean SUBTRACT operation (polygon set difference) of two geometry sets
 * @param[in] begin1 iterator to the beginning of the geometry collection one
 * @param[in] end1 iterator to the ending of the geometry collection one
 * @param[in] begin2 iterator to the beginning of the geometry collection two
 * @param[in] end2 iterator to the ending of the geometry collection two
 * @param[out] results stl container with Polygon2D that hold the difference result of two geometry sets
 */
template <typename iterator1, typename iterator2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator1>::value_type>::value &&
                                  traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator2>::value_type>::value, bool>::type = true>
inline void Subtract(iterator1 begin1, iterator1 end1, iterator2 begin2, iterator2 end2,
                     container<polygon_type<typename std::iterator_traits<iterator1>::value_type>,
                     allocator<polygon_type<typename std::iterator_traits<iterator1>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator1>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet;
    for(auto iter = begin1; iter != end1; ++iter)
        polygonSet += *iter;
    for(auto iter = begin2; iter != end2; ++iter)
        polygonSet -= *iter;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean XOR operation (polygon set disjoint-union) of two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the disjoint-union result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Xor(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet ^= g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean XOR operation (polygon set disjoint-union) of two geometry sets
 * @param[in] begin1 iterator to the beginning of the geometry collection one
 * @param[in] end1 iterator to the ending of the geometry collection one
 * @param[in] begin2 iterator to the beginning of the geometry collection two
 * @param[in] end2 iterator to the ending of the geometry collection two
 * @param[out] results stl container with Polygon2D that hold the disjoint-union result of two geometry sets
 */
template <typename iterator1, typename iterator2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator1>::value_type>::value &&
                                  traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator2>::value_type>::value, bool>::type = true>
inline void Xor(iterator1 begin1, iterator1 end1, iterator2 begin2, iterator2 end2,
                container<polygon_type<typename std::iterator_traits<iterator1>::value_type>,
                allocator<polygon_type<typename std::iterator_traits<iterator1>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator1>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet1, polygonSet2;
    for(auto iter1 = begin1; iter1 != end1; ++iter1)
        polygonSet1 += *iter1;
    for(auto iter2 = begin2; iter2 != end2; ++iter2)
        polygonSet2 += *iter2;
    polygonSet1 ^= polygonSet2;
    results.clear();
    polygonSet1.get(results);
}

} //namespace boolean
} //namespace geometry
} //namespace generic
#endif//GENERIC_GEOMETRY_BOOLEANOPERATION_HPP


//  (C) Copyright John Maddock 2005.
//  (C) Copyright Henry S. Warren 2005.
//  Use, modification and distribution are 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_TR1_RANDOM_HPP_INCLUDED
#  define BOOST_TR1_RANDOM_HPP_INCLUDED
#  include <boost/tr1/detail/config.hpp>

#ifdef BOOST_HAS_TR1_RANDOM
#  if defined(BOOST_HAS_INCLUDE_NEXT) && !defined(BOOST_TR1_DISABLE_INCLUDE_NEXT)
#     include_next BOOST_TR1_HEADER(random)
#  else
#     include <boost/tr1/detail/config_all.hpp>
#     include BOOST_TR1_STD_HEADER(BOOST_TR1_PATH(random))
#  endif
#else
// Boost.Random:
#include <boost/random.hpp>
#ifndef __SUNPRO_CC
    // Sunpros linker complains if we so much as include this...
#   include <boost/nondet_random.hpp>
#endif
#include <boost/tr1/detail/functor2iterator.hpp>
#include <boost/type_traits/is_fundamental.hpp>
#include <boost/type_traits/is_same.hpp>

namespace std { namespace tr1{

using ::boost::variate_generator;

template<class UIntType, UIntType a, UIntType c, UIntType m>
class linear_congruential
{
private:
   typedef ::boost::random::linear_congruential<UIntType, a, c, m, 0> impl_type;
public:
   // types
   typedef UIntType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(UIntType, multiplier = a);
   BOOST_STATIC_CONSTANT(UIntType, increment = c);
   BOOST_STATIC_CONSTANT(UIntType, modulus = m);
   // constructors and member function
   explicit linear_congruential(unsigned long x0 = 1)
      : m_gen(x0){}
   linear_congruential(const linear_congruential& that)
      : m_gen(that.m_gen){}
   template<class Gen> linear_congruential(Gen& g)
   {
      init1(g, ::boost::is_same<Gen,linear_congruential>());
   }
   void seed(unsigned long x0 = 1)
   { m_gen.seed(x0); }
   template<class Gen> void seed(Gen& g)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   {
      return m_gen();
   }
   bool operator==(const linear_congruential& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const linear_congruential& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os,
             const linear_congruential& lcg)
  {
    return os << lcg.m_gen;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is,
             linear_congruential& lcg)
  {
    return is >> lcg.m_gen;
  }
#endif

private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      //typedef typename Gen::result_type gen_rt;
      boost::tr1_details::functor2iterator<Gen, unsigned long> f1(g), f2;
      m_gen.seed(f1, f2);
   }
   impl_type m_gen;
};

template<class UIntType, int w, int n, int m, int r,
UIntType a, int u, int s, UIntType b, int t, UIntType c, int l>
class mersenne_twister
{
   typedef ::boost::random::mersenne_twister
      <UIntType, w, n, m, r, a, u, s, b, t, c, l, 0> imp_type;
public:
   // types
   typedef UIntType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(int, word_size = w);
   BOOST_STATIC_CONSTANT(int, state_size = n);
   BOOST_STATIC_CONSTANT(int, shift_size = m);
   BOOST_STATIC_CONSTANT(int, mask_bits = r);
   BOOST_STATIC_CONSTANT(UIntType, parameter_a = a);
   BOOST_STATIC_CONSTANT(int, output_u = u);
   BOOST_STATIC_CONSTANT(int, output_s = s);
   BOOST_STATIC_CONSTANT(UIntType, output_b = b);
   BOOST_STATIC_CONSTANT(int, output_t = t);
   BOOST_STATIC_CONSTANT(UIntType, output_c = c);
   BOOST_STATIC_CONSTANT(int, output_l = l);
   // constructors and member function
   mersenne_twister(){}
   explicit mersenne_twister(unsigned long value)
      : m_gen(value == 0 ? 5489UL : value){}
   template<class Gen> mersenne_twister(Gen& g)
   {
      init1(g, ::boost::is_same<mersenne_twister,Gen>());
   }
   void seed()
   { m_gen.seed(); }
   void seed(unsigned long value)
   { m_gen.seed(value == 0 ? 5489UL : value); }
   template<class Gen> void seed(Gen& g)
   { init2(g, ::boost::is_fundamental<Gen>()); }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   { return m_gen(); }
   bool operator==(const mersenne_twister& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const mersenne_twister& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const mersenne_twister& lcg)
   {
      return os << lcg.m_gen;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            mersenne_twister& lcg)
   {
      return is >> lcg.m_gen;
   }
#endif
private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g == 0 ? 4357UL : g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      m_gen.seed(g);
   }
   imp_type m_gen;
};

template<class IntType, IntType m, int s, int r>
class subtract_with_carry
{
public:
   // types
   typedef IntType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(IntType, modulus = m);
   BOOST_STATIC_CONSTANT(int, long_lag = r);
   BOOST_STATIC_CONSTANT(int, short_lag = s);

   // constructors and member function
   subtract_with_carry(){}
   explicit subtract_with_carry(unsigned long value)
      : m_gen(value == 0 ? 19780503UL : value){}
   template<class Gen> subtract_with_carry(Gen& g)
   { init1(g, ::boost::is_same<Gen, subtract_with_carry<IntType, m, s, r> >()); }
   void seed(unsigned long value = 19780503ul)
   { m_gen.seed(value == 0 ? 19780503UL : value); }
   template<class Gen> void seed(Gen& g)
   { init2(g, ::boost::is_fundamental<Gen>()); }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   { return m_gen(); }
   bool operator==(const subtract_with_carry& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const subtract_with_carry& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const subtract_with_carry& lcg)
   {
      return os << lcg.m_gen;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            subtract_with_carry& lcg)
   {
      return is >> lcg.m_gen;
   }
#endif
private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g == 0 ? 19780503UL : g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      m_gen.seed(g);
   }
   ::boost::random::subtract_with_carry<IntType, m, s, r, 0> m_gen;
};

template<class RealType, int w, int s, int r>
class subtract_with_carry_01
{
public:
   // types
   typedef RealType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(int, word_size = w);
   BOOST_STATIC_CONSTANT(int, long_lag = r);
   BOOST_STATIC_CONSTANT(int, short_lag = s);

   // constructors and member function
   subtract_with_carry_01(){}
   explicit subtract_with_carry_01(unsigned long value)
      : m_gen(value == 0 ? 19780503UL : value){}
   template<class Gen> subtract_with_carry_01(Gen& g)
   { init1(g, ::boost::is_same<Gen, subtract_with_carry_01<RealType, w, s, r> >()); }
   void seed(unsigned long value = 19780503UL)
   { m_gen.seed(value == 0 ? 19780503UL : value); }
   template<class Gen> void seed(Gen& g)
   { init2(g, ::boost::is_fundamental<Gen>()); }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   { return m_gen(); }
   bool operator==(const subtract_with_carry_01& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const subtract_with_carry_01& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const subtract_with_carry_01& lcg)
   {
      return os << lcg.m_gen;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            subtract_with_carry_01& lcg)
   {
      return is >> lcg.m_gen;
   }
#endif
private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g == 0 ? 19780503UL : g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      //typedef typename Gen::result_type gen_rt;
      boost::tr1_details::functor2iterator<Gen, unsigned long> f1(g), f2;
      m_gen.seed(f1, f2);
   }
   ::boost::random::subtract_with_carry_01<RealType, w, s, r, 0> m_gen;
};

using ::boost::random::discard_block;

template<class UniformRandomNumberGenerator1, int s1, class UniformRandomNumberGenerator2, int s2>
class xor_combine
{
public:
   // types
   typedef UniformRandomNumberGenerator1 base1_type;
   typedef UniformRandomNumberGenerator2 base2_type;
   typedef unsigned long result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(int, shift1 = s1);
   BOOST_STATIC_CONSTANT(int, shift2 = s2);
   // constructors and member function
   xor_combine(){ init_minmax(); }
   xor_combine(const base1_type & rng1, const base2_type & rng2)
      : m_b1(rng1), m_b2(rng2) { init_minmax(); }
   xor_combine(unsigned long s)
      : m_b1(s), m_b2(s+1) { init_minmax(); }
   template<class Gen> xor_combine(Gen& g)
   {
      init_minmax();
      init1(g, ::boost::is_same<Gen, xor_combine<UniformRandomNumberGenerator1, s1, UniformRandomNumberGenerator2, s2> >());
   }
   void seed()
   {
      m_b1.seed();
      m_b2.seed();
   }
   void seed(unsigned long s)
   {
      m_b1.seed(s);
      m_b2.seed(s+1);
   }
   template<class Gen> void seed(Gen& g)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }

   const base1_type& base1() const
   { return m_b1; }
   const base2_type& base2() const
   { return m_b2; }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return m_min; }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return m_max; }
   result_type operator()()
   { return (m_b1() << s1) ^ (m_b2() << s2); }

   bool operator == (const xor_combine& that)const
   { return (m_b1 == that.m_b1) && (m_b2 == that.m_b2); }
   bool operator != (const xor_combine& that)const
   { return !(*this == that); }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const xor_combine& lcg)
   {
      return os << lcg.m_b1 << " " << lcg.m_b2;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            xor_combine& lcg)
   {
      return is >> lcg.m_b1 >> lcg.m_b2;
   }
#endif

private:
   void init_minmax();
   base1_type m_b1;
   base2_type m_b2;
   result_type m_min;
   result_type m_max;

   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_b1 = g.m_b1;
      m_b2 = g.m_b2;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_b1.seed(static_cast<unsigned long>(g));
      m_b2.seed(static_cast<unsigned long>(g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      m_b1.seed(g);
      m_b2.seed(g);
   }
};

template<class UniformRandomNumberGenerator1, int s1, class UniformRandomNumberGenerator2, int s2>
void xor_combine<UniformRandomNumberGenerator1, s1, UniformRandomNumberGenerator2, s2>::init_minmax()
{
   //
   // The following code is based on that given in "Hacker's Delight"
   // by Henry S. Warren, (Addison-Wesley, 2003), and at
   // http://www.hackersdelight.org/index.htm.
   // Used here by permission.
   //
   // calculation of minimum value:
   //
   result_type a = (m_b1.min)() << s1;
   result_type b = (m_b1.max)() << s1;
   result_type c = (m_b2.min)() << s2;
   result_type d = (m_b2.max)() << s2;
   result_type m, temp;

   m = 0x1uL << ((sizeof(result_type) * CHAR_BIT) - 1);
   while (m != 0) {
      if (~a & c & m) {
         temp = (a | m) & (static_cast<result_type>(0u) - m);
         if (temp <= b) a = temp;
      }
      else if (a & ~c & m) {
         temp = (c | m) & (static_cast<result_type>(0u) - m);
         if (temp <= d) c = temp;
      }
      m >>= 1;
   }
   m_min = a ^ c;

   //
   // calculation of maximum value:
   //
   if((((std::numeric_limits<result_type>::max)() >> s1) < (m_b1.max)())
      || ((((std::numeric_limits<result_type>::max)()) >> s2) < (m_b2.max)()))
   {
      m_max = (std::numeric_limits<result_type>::max)();
      return;
   }
   a = (m_b1.min)() << s1;
   b = (m_b1.max)() << s1;
   c = (m_b2.min)() << s2;
   d = (m_b2.max)() << s2;

   m = 0x1uL << ((sizeof(result_type) * CHAR_BIT) - 1);

   while (m != 0) {
      if (b & d & m) {
         temp = (b - m) | (m - 1);
         if (temp >= a) b = temp;
         else {
            temp = (d - m) | (m - 1);
            if (temp >= c) d = temp;
         }
      }
      m = m >> 1;
   }
   m_max = b ^ d;
}

typedef linear_congruential< ::boost::int32_t, 16807, 0, 2147483647> minstd_rand0;
typedef linear_congruential< ::boost::int32_t, 48271, 0, 2147483647> minstd_rand;
typedef mersenne_twister< ::boost::uint32_t, 32,624,397,31,0x9908b0df,11,7,0x9d2c5680,15,0xefc60000,18> mt19937;
typedef subtract_with_carry_01<float, 24, 10, 24> ranlux_base_01;
typedef subtract_with_carry_01<double, 48, 10, 24> ranlux64_base_01;
typedef discard_block<subtract_with_carry< ::boost::int32_t, (1<<24), 10, 24>, 223, 24> ranlux3;
typedef discard_block<subtract_with_carry< ::boost::int32_t, (1<<24), 10, 24>, 389, 24> ranlux4;
typedef discard_block<subtract_with_carry_01<float, 24, 10, 24>, 223, 24> ranlux3_01;
typedef discard_block<subtract_with_carry_01<float, 24, 10, 24>, 389, 24> ranlux4_01;

#ifndef __SUNPRO_CC
using ::boost::random_device;
#endif
using ::boost::uniform_int;

class bernoulli_distribution
{
public:
   // types
   typedef int input_type;
   typedef bool result_type;
   // constructors and member function
   explicit bernoulli_distribution(double p = 0.5)
      : m_dist(p){}
   double p() const
   { return m_dist.p(); }
   void reset()
   { m_dist.reset(); }
   template<class UniformRandomNumberGenerator>
   result_type operator()(UniformRandomNumberGenerator& urng)
   {
      return m_dist(urng);
   }
#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const bernoulli_distribution& lcg)
   {
      return os << lcg.m_dist;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            bernoulli_distribution& lcg)
   {
      return is >> lcg.m_dist;
   }
#endif

private:
   ::boost::bernoulli_distribution<double> m_dist;
};
//using ::boost::bernoulli_distribution;
using ::boost::geometric_distribution;
using ::boost::poisson_distribution;
using ::boost::binomial_distribution;
using ::boost::uniform_real;
using ::boost::exponential_distribution;
using ::boost::normal_distribution;
using ::boost::gamma_distribution;

} }

#endif

#endif


#include <sm/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>

namespace sm {

namespace random {

class Random
{
 public:
  // http://www.boost.org/doc/libs/1_49_0/doc/html/boost_random/reference.html#boost_random.reference.generators
  typedef boost::mt19937 base_generator_type;

  base_generator_type _generator;
  // Define a uniform random number distribution which produces "double"
  // values between 0 and 1 (0 inclusive, 1 exclusive).
  boost::uniform_real<> _uni_dist;
  boost::variate_generator<base_generator_type&, boost::uniform_real<> > _uniform;
  boost::normal_distribution<> _normal_dist;
  boost::variate_generator<base_generator_type&, boost::normal_distribution<> > _normal;

  Random() : _uni_dist(0.0,1.0), _uniform(_generator, _uni_dist), _normal_dist(), _normal(_generator, _normal_dist)
  {
	// 0
  }

  double normal()
  {
	return _normal();
  }

  double uniform()
  {
	return _uniform();
  }

  void seed(boost::uint64_t s)
  {
    _normal.engine().seed(s);
    _normal.distribution().reset();
    _uniform.engine().seed(s);
    _uniform.distribution().reset();
  }

  static Random & instance()
  {
	static Random random;
	return random;
  }
};

double normal()
{
  return Random::instance().normal();
}

double randn()
{
  return Random::instance().normal();
}

double uniform()
{
  return Random::instance().uniform();
}

double rand()
{
  return Random::instance().uniform();
}

double randLU(double lowerBoundInclusive, double upperBoundExclusive)
{
  return Random::instance().uniform() * (upperBoundExclusive - lowerBoundInclusive) + lowerBoundInclusive;
}

int randLUi(int lowerBoundInclusive, int upperBoundExclusive)
{
  return (int)floor(Random::instance().uniform() * (upperBoundExclusive - lowerBoundInclusive)) + lowerBoundInclusive;
}

void seed(boost::uint64_t s)
{
  Random::instance().seed(s);
}

} // namespace random

} // namespace sm


/* compile with g++ ... -l arprec */

#include <iostream>
#include <fstream>
#include <arprec/mp_real.h>
#include <arprec/mp_complex.h>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>
#include <cassert>
#include <boost/numeric/linear_algebra/identity.hpp>

using namespace std;
typedef mtl::dense2D<mp_complex> mat_c;

ostream& operator<< (ostream& os, const mp_complex& c) {
	return os << "(" << c.real << ", " << c.imag << ")";
}

int main() {

	mp::mp_init(102);
	cout.precision(30);

	mp_real x1,x2,x3,x4,x5,x6;
	x1 = mp_real("2e18");
	x2 = mp_real("-1.34");
	x3 = mp_real("1.6");
	x4 = mp_real(3);
	x5 = mp_real("1.83e7");
	x6 = mp_real("-4.234");
	cout << "x1 = " << x1 << "\n";
	cout << "x2 = " << x2 << "\n";
	cout << "x3 = " << x3 << "\n";
	cout << "x4 = " << x4 << "\n";
	cout << "x5 = " << x5 << "\n";
	cout << "x6 = " << x6 << "\n";
	cout << "---------------\n";

	mp_complex z1,z2,z3,z4,z5,z6;
	z1 = mp_complex("4e30","3e30");
	z2 = mp_complex("3","-3.5");
	z3 = z1+z2;
	z4 = sqrt(mp_complex("-4","0"));
	z5 = z2+z4;
	z6 = z2-z3+z1+z4;
	cout << "z1 = " << z1 << "\n";
	cout << "z2 = " << z2 << "\n";
	cout << "z3 = " << z3 << "\n";
	cout << "z4 = " << z4 << "\n";
	cout << "z5 = " << z5 << "\n";
	cout << "z6 = " << z6 << "\n";
	cout << "---------------\n";
	{
	mtl::dense2D<mp_real> A(2,2),B(2,2),C(2,2),D(2,2);
	mtl::dense_vector<mp_real> b(2),x(2),r(2);
	A = x1,x2,
	    x3,x4;
	b = x5,x6;
	cout << "A = \n" << A;
	cout << "b = " << b << endl;

	// A = A*A;	// Laufzeitfehler
	A*=A;
	cout << "A = \n" << A;
	cout << "A*A = \n" << A*A;
	B =A*A;
	cout << "B = \n" << B;
	C = trans(A);
	cout << "C = \n" << C;
	D = A;

	mtl::dense_vector<int> p(2);
	lu(A,p);
	x = lu_apply(A,p,b);
	r = D*x-b;
	cout << "Ax-b = " << r << endl;
	cout << "---------------\n";
	}

	{
	mtl::dense2D<mp_complex> A(2,2),B(2,2),C(2,2),D(2,2);
	mtl::dense_vector<mp_complex> b(2),x(2),r(2);
 	A = z1,z2,
 	    z3,z4;

	b = z5,z6;
	cout << "A = \n" << A;
	cout << "b = " << b << endl;

	// A = A*A;	// Compilezeitfehler
	// A*=A;	// Compilezeitfehler
	B =A*A;	// Compilezeitfehler
	C = trans(A);	// Compilezeitfehler

	D = A;

	mtl::dense_vector<int> p(2);
	lu(A,p);	// Compilezeitfehler
	x = lu_apply(A,p,b);
	r = D*x-b;
 	cout << "Ax-b = " << r << endl;
	cout << "---------------\n";

	cout << "abs(A[0][0]) is " << abs(A[0][0]) << '\n';
	}

	mp::mp_finalize();

}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are 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_MATH_TOOLS_REMEZ_HPP
#define BOOST_MATH_TOOLS_REMEZ_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/solve.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/function/function1.hpp>
#include <boost/scoped_array.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{ namespace math{ namespace tools{

namespace detail{

//
// The error function: the difference between F(x) and
// the current approximation.  This is the function
// for which we must find the extema.
//
template <class T>
struct remez_error_function
{
   typedef boost::function1<T, T const &> function_type;
public:
   remez_error_function(
      function_type f_,
      const polynomial<T>& n,
      const polynomial<T>& d,
      bool rel_err)
         : f(f_), numerator(n), denominator(d), rel_error(rel_err) {}

   T operator()(const T& z)const
   {
      T y = f(z);
      T abs = y - (numerator.evaluate(z) / denominator.evaluate(z));
      T err;
      if(rel_error)
      {
         if(y != 0)
            err = abs / fabs(y);
         else if(0 == abs)
         {
            // we must be at a root, or it's not recoverable:
            BOOST_ASSERT(0 == abs);
            err = 0;
         }
         else
         {
            // We have a divide by zero!
            // Lets assume that f(x) is zero as a result of
            // internal cancellation error that occurs as a result
            // of shifting a root at point z to the origin so that
            // the approximation can be "pinned" to pass through
            // the origin: in that case it really
            // won't matter what our approximation calculates here
            // as long as it's a small number, return the absolute error:
            err = abs;
         }
      }
      else
         err = abs;
      return err;
   }
private:
   function_type f;
   polynomial<T> numerator;
   polynomial<T> denominator;
   bool rel_error;
};
//
// This function adapts the error function so that it's minima
// are the extema of the error function.  We can find the minima
// with standard techniques.
//
template <class T>
struct remez_max_error_function
{
   remez_max_error_function(const remez_error_function<T>& f)
      : func(f) {}

   T operator()(const T& x)
   {
      BOOST_MATH_STD_USING
      return -fabs(func(x));
   }
private:
   remez_error_function<T> func;
};

} // detail

template <class T>
class remez_minimax
{
public:
   typedef boost::function1<T, T const &> function_type;
   typedef boost::numeric::ublas::vector<T> vector_type;
   typedef boost::numeric::ublas::matrix<T> matrix_type;

   remez_minimax(function_type f, unsigned oN, unsigned oD, T a, T b, bool pin = true, bool rel_err = false, int sk = 0, int bits = 0);
   remez_minimax(function_type f, unsigned oN, unsigned oD, T a, T b, bool pin, bool rel_err, int sk, int bits, const vector_type& points);

   void reset(unsigned oN, unsigned oD, T a, T b, bool pin = true, bool rel_err = false, int sk = 0, int bits = 0);
   void reset(unsigned oN, unsigned oD, T a, T b, bool pin, bool rel_err, int sk, int bits, const vector_type& points);

   void set_brake(int b)
   {
      BOOST_ASSERT(b < 100);
      BOOST_ASSERT(b >= 0);
      m_brake = b;
   }

   T iterate();

   polynomial<T> denominator()const;
   polynomial<T> numerator()const;

   vector_type const& chebyshev_points()const
   {
      return control_points;
   }

   vector_type const& zero_points()const
   {
      return zeros;
   }

   T error_term()const
   {
      return solution[solution.size() - 1];
   }
   T max_error()const
   {
      return m_max_error;
   }
   T max_change()const
   {
      return m_max_change;
   }
   void rotate()
   {
      --orderN;
      ++orderD;
   }
   void rescale(T a, T b)
   {
      T scale = (b - a) / (max - min);
      for(unsigned i = 0; i < control_points.size(); ++i)
      {
         control_points[i] = (control_points[i] - min) * scale + a;
      }
      min = a;
      max = b;
   }
private:

   void init_chebyshev();

   function_type func;            // The function to approximate.
   vector_type control_points;    // Current control points to be used for the next iteration.
   vector_type solution;          // Solution from the last iteration contains all unknowns including the error term.
   vector_type zeros;             // Location of points of zero error from last iteration, plus the two end points.
   vector_type maxima;            // Location of maxima of the error function, actually contains the control points used for the last iteration.
   T m_max_error;                 // Maximum error found in last approximation.
   T m_max_change;                // Maximum change in location of control points after last iteration.
   unsigned orderN;               // Order of the numerator polynomial.
   unsigned orderD;               // Order of the denominator polynomial.
   T min, max;                    // End points of the range to optimise over.
   bool rel_error;                // If true optimise for relative not absolute error.
   bool pinned;                   // If true the approximation is "pinned" to go through the origin.
   unsigned unknowns;             // Total number of unknowns.
   int m_precision;               // Number of bits precision to which the zeros and maxima are found.
   T m_max_change_history[2];     // Past history of changes to control points.
   int m_brake;                     // amount to break by in percentage points.
   int m_skew;                      // amount to skew starting points by in percentage points: -100-100
};

#ifndef BRAKE
#define BRAKE 0
#endif
#ifndef SKEW
#define SKEW 0
#endif

template <class T>
void remez_minimax<T>::init_chebyshev()
{
   BOOST_MATH_STD_USING
   //
   // Fill in the zeros:
   //
   unsigned terms = pinned ? orderD + orderN : orderD + orderN + 1;

   for(unsigned i = 0; i < terms; ++i)
   {
      T cheb = cos((2 * terms - 1 - 2 * i) * constants::pi<T>() / (2 * terms));
      cheb += 1;
      cheb /= 2;
      if(m_skew != 0)
      {
         T p = static_cast<T>(200 + m_skew) / 200;
         cheb = pow(cheb, p);
      }
      cheb *= (max - min);
      cheb += min;
      zeros[i+1] = cheb;
   }
   zeros[0] = min;
   zeros[unknowns] = max;
   // perform a regular interpolation fit:
   matrix_type A(terms, terms);
   vector_type b(terms);
   // fill in the y values:
   for(unsigned i = 0; i < b.size(); ++i)
   {
      b[i] = func(zeros[i+1]);
   }
   // fill in powers of x evaluated at each of the control points:
   unsigned offsetN = pinned ? 0 : 1;
   unsigned offsetD = offsetN + orderN;
   unsigned maxorder = (std::max)(orderN, orderD);
   for(unsigned i = 0; i < b.size(); ++i)
   {
      T x0 = zeros[i+1];
      T x = x0;
      if(!pinned)
         A(i, 0) = 1;
      for(unsigned j = 0; j < maxorder; ++j)
      {
         if(j < orderN)
            A(i, j + offsetN) = x;
         if(j < orderD)
         {
            A(i, j + offsetD) = -x * b[i];
         }
         x *= x0;
      }
   }
   //
   // Now go ahead and solve the expression to get our solution:
   //
   vector_type l_solution = boost::math::tools::solve(A, b);
   // need to add a "fake" error term:
   l_solution.resize(unknowns);
   l_solution[unknowns-1] = 0;
   solution = l_solution;
   //
   // Now find all the extrema of the error function:
   //
   detail::remez_error_function<T> Err(func, this->numerator(), this->denominator(), rel_error);
   detail::remez_max_error_function<T> Ex(Err);
   m_max_error = 0;
   //int max_err_location = 0;
   for(unsigned i = 0; i < unknowns; ++i)
   {
      std::pair<T, T> r = brent_find_minima(Ex, zeros[i], zeros[i+1], m_precision);
      maxima[i] = r.first;
      T rel_err = fabs(r.second);
      if(rel_err > m_max_error)
      {
         m_max_error = fabs(r.second);
         //max_err_location = i;
      }
   }
   control_points = maxima;
}

template <class T>
void remez_minimax<T>::reset(
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits)
{
   control_points = vector_type(oN + oD + (pin ? 1 : 2));
   solution = control_points;
   zeros = vector_type(oN + oD + (pin ? 2 : 3));
   maxima = control_points;
   orderN = oN;
   orderD = oD;
   rel_error = rel_err;
   pinned = pin;
   m_skew = sk;
   min = a;
   max = b;
   m_max_error = 0;
   unknowns = orderN + orderD + (pinned ? 1 : 2);
   // guess our initial control points:
   control_points[0] = min;
   control_points[unknowns - 1] = max;
   T interval = (max - min) / (unknowns - 1);
   T spot = min + interval;
   for(unsigned i = 1; i < control_points.size(); ++i)
   {
      control_points[i] = spot;
      spot += interval;
   }
   solution[unknowns - 1] = 0;
   m_max_error = 0;
   if(bits == 0)
   {
      // don't bother about more than float precision:
      m_precision = (std::min)(24, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   else
   {
      // can't be more accurate than half the bits of T:
      m_precision = (std::min)(bits, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   m_max_change_history[0] = m_max_change_history[1] = 1;
   init_chebyshev();
   // do one iteration whatever:
   //iterate();
}

template <class T>
inline remez_minimax<T>::remez_minimax(
         typename remez_minimax<T>::function_type f,
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits)
   : func(f)
{
   m_brake = 0;
   reset(oN, oD, a, b, pin, rel_err, sk, bits);
}

template <class T>
void remez_minimax<T>::reset(
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits,
         const vector_type& points)
{
   control_points = vector_type(oN + oD + (pin ? 1 : 2));
   solution = control_points;
   zeros = vector_type(oN + oD + (pin ? 2 : 3));
   maxima = control_points;
   orderN = oN;
   orderD = oD;
   rel_error = rel_err;
   pinned = pin;
   m_skew = sk;
   min = a;
   max = b;
   m_max_error = 0;
   unknowns = orderN + orderD + (pinned ? 1 : 2);
   control_points = points;
   solution[unknowns - 1] = 0;
   m_max_error = 0;
   if(bits == 0)
   {
      // don't bother about more than float precision:
      m_precision = (std::min)(24, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   else
   {
      // can't be more accurate than half the bits of T:
      m_precision = (std::min)(bits, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   m_max_change_history[0] = m_max_change_history[1] = 1;
   // do one iteration whatever:
   //iterate();
}

template <class T>
inline remez_minimax<T>::remez_minimax(
         typename remez_minimax<T>::function_type f,
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits,
         const vector_type& points)
   : func(f)
{
   m_brake = 0;
   reset(oN, oD, a, b, pin, rel_err, sk, bits, points);
}

template <class T>
T remez_minimax<T>::iterate()
{
   BOOST_MATH_STD_USING
   matrix_type A(unknowns, unknowns);
   vector_type b(unknowns);

   // fill in evaluation of f(x) at each of the control points:
   for(unsigned i = 0; i < b.size(); ++i)
   {
      // take care that none of our control points are at the origin:
      if(pinned && (control_points[i] == 0))
      {
         if(i)
            control_points[i] = control_points[i-1] / 3;
         else
            control_points[i] = control_points[i+1] / 3;
      }
      b[i] = func(control_points[i]);
   }

   T err_err;
   unsigned convergence_count = 0;
   do{
      // fill in powers of x evaluated at each of the control points:
      int sign = 1;
      unsigned offsetN = pinned ? 0 : 1;
      unsigned offsetD = offsetN + orderN;
      unsigned maxorder = (std::max)(orderN, orderD);
      T Elast = solution[unknowns - 1];

      for(unsigned i = 0; i < b.size(); ++i)
      {
         T x0 = control_points[i];
         T x = x0;
         if(!pinned)
            A(i, 0) = 1;
         for(unsigned j = 0; j < maxorder; ++j)
         {
            if(j < orderN)
               A(i, j + offsetN) = x;
            if(j < orderD)
            {
               T mult = rel_error ? T(b[i] - sign * fabs(b[i]) * Elast): T(b[i] - sign * Elast);
               A(i, j + offsetD) = -x * mult;
            }
            x *= x0;
         }
         // The last variable to be solved for is the error term,
         // sign changes with each control point:
         T E = rel_error ? T(sign * fabs(b[i])) : T(sign);
         A(i, unknowns - 1) = E;
         sign = -sign;
      }

   #ifdef BOOST_MATH_INSTRUMENT
      for(unsigned i = 0; i < b.size(); ++i)
         std::cout << b[i] << " ";
      std::cout << "\n\n";
      for(unsigned i = 0; i < b.size(); ++i)
      {
         for(unsigned j = 0; j < b.size(); ++ j)
            std::cout << A(i, j) << " ";
         std::cout << "\n";
      }
      std::cout << std::endl;
   #endif
      //
      // Now go ahead and solve the expression to get our solution:
      //
      solution = boost::math::tools::solve(A, b);

      err_err = (Elast != 0) ? T(fabs((fabs(solution[unknowns-1]) - fabs(Elast)) / fabs(Elast))) : T(1);
   }while(orderD && (convergence_count++ < 80) && (err_err > 0.001));

   //
   // Perform a sanity check to verify that the solution to the equations
   // is not so much in error as to be useless.  The matrix inversion can
   // be very close to singular, so this can be a real problem.
   //
   vector_type sanity = prod(A, solution);
   for(unsigned i = 0; i < b.size(); ++i)
   {
      T err = fabs((b[i] - sanity[i]) / fabs(b[i]));
      if(err > sqrt(epsilon<T>()))
      {
         std::cerr << "Sanity check failed: more than half the digits in the found solution are in error." << std::endl;
      }
   }

   //
   // Next comes another sanity check, we want to verify that all the control
   // points do actually alternate in sign, in practice we may have
   // additional roots in the error function that cause this to fail.
   // Failure here is always fatal: even though this code attempts to correct
   // the problem it usually only postpones the inevitable.
   //
   polynomial<T> num, denom;
   num = this->numerator();
   denom = this->denominator();
   T e1 = b[0] - num.evaluate(control_points[0]) / denom.evaluate(control_points[0]);
#ifdef BOOST_MATH_INSTRUMENT
   std::cout << e1;
#endif
   for(unsigned i = 1; i < b.size(); ++i)
   {
      T e2 = b[i] - num.evaluate(control_points[i]) / denom.evaluate(control_points[i]);
#ifdef BOOST_MATH_INSTRUMENT
      std::cout << " " << e2;
#endif
      if(e2 * e1 > 0)
      {
         std::cerr << std::flush << "Basic sanity check failed: Error term does not alternate in sign, non-recoverable error may follow..." << std::endl;
         T perturbation = 0.05;
         do{
            T point = control_points[i] * (1 - perturbation) + control_points[i-1] * perturbation;
            e2 = func(point) - num.evaluate(point) / denom.evaluate(point);
            if(e2 * e1 < 0)
            {
               control_points[i] = point;
               break;
            }
            perturbation += 0.05;
         }while(perturbation < 0.8);

         if((e2 * e1 > 0) && (i + 1 < b.size()))
         {
            perturbation = 0.05;
            do{
               T point = control_points[i] * (1 - perturbation) + control_points[i+1] * perturbation;
               e2 = func(point) - num.evaluate(point) / denom.evaluate(point);
               if(e2 * e1 < 0)
               {
                  control_points[i] = point;
                  break;
               }
               perturbation += 0.05;
            }while(perturbation < 0.8);
         }

      }
      e1 = e2;
   }

#ifdef BOOST_MATH_INSTRUMENT
   for(unsigned i = 0; i < solution.size(); ++i)
      std::cout << solution[i] << " ";
   std::cout << std::endl << this->numerator() << std::endl;
   std::cout << this->denominator() << std::endl;
   std::cout << std::endl;
#endif

   //
   // The next step is to find all the intervals in which our maxima
   // lie:
   //
   detail::remez_error_function<T> Err(func, this->numerator(), this->denominator(), rel_error);
   zeros[0] = min;
   zeros[unknowns] = max;
   for(unsigned i = 1; i < control_points.size(); ++i)
   {
      eps_tolerance<T> tol(m_precision);
      boost::uintmax_t max_iter = 1000;
      std::pair<T, T> p = toms748_solve(
         Err,
         control_points[i-1],
         control_points[i],
         tol,
         max_iter);
      zeros[i] = (p.first + p.second) / 2;
      //zeros[i] = bisect(Err, control_points[i-1], control_points[i], m_precision);
   }
   //
   // Now find all the extrema of the error function:
   //
   detail::remez_max_error_function<T> Ex(Err);
   m_max_error = 0;
   //int max_err_location = 0;
   for(unsigned i = 0; i < unknowns; ++i)
   {
      std::pair<T, T> r = brent_find_minima(Ex, zeros[i], zeros[i+1], m_precision);
      maxima[i] = r.first;
      T rel_err = fabs(r.second);
      if(rel_err > m_max_error)
      {
         m_max_error = fabs(r.second);
         //max_err_location = i;
      }
   }
   //
   // Almost done now! we just need to set our control points
   // to the extrema, and calculate how much each point has changed
   // (this will be our termination condition):
   //
   swap(control_points, maxima);
   m_max_change = 0;
   //int max_change_location = 0;
   for(unsigned i = 0; i < unknowns; ++i)
   {
      control_points[i] = (control_points[i] * (100 - m_brake) + maxima[i] * m_brake) / 100;
      T change = fabs((control_points[i] - maxima[i]) / control_points[i]);
#if 0
      if(change > m_max_change_history[1])
      {
         // divergence!!! try capping the change:
         std::cerr << "Possible divergent step, change will be capped!!" << std::endl;
         change = m_max_change_history[1];
         if(control_points[i] < maxima[i])
            control_points[i] = maxima[i] - change * maxima[i];
         else
            control_points[i] = maxima[i] + change * maxima[i];
      }
#endif
      if(change > m_max_change)
      {
         m_max_change = change;
         //max_change_location = i;
      }
   }
   //
   // store max change information:
   //
   m_max_change_history[0] = m_max_change_history[1];
   m_max_change_history[1] = fabs(m_max_change);

   return m_max_change;
}

template <class T>
polynomial<T> remez_minimax<T>::numerator()const
{
   boost::scoped_array<T> a(new T[orderN + 1]);
   if(pinned)
      a[0] = 0;
   unsigned terms = pinned ? orderN : orderN + 1;
   for(unsigned i = 0; i < terms; ++i)
      a[pinned ? i+1 : i] = solution[i];
   return boost::math::tools::polynomial<T>(&a[0], orderN);
}

template <class T>
polynomial<T> remez_minimax<T>::denominator()const
{
   unsigned terms = orderD + 1;
   unsigned offsetD = pinned ? orderN : (orderN + 1);
   boost::scoped_array<T> a(new T[terms]);
   a[0] = 1;
   for(unsigned i = 0; i < orderD; ++i)
      a[i+1] = solution[i + offsetD];
   return boost::math::tools::polynomial<T>(&a[0], orderD);
}

}}} // namespaces

#endif // BOOST_MATH_TOOLS_REMEZ_HPP


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_second_order_cone_hpp__
#define __multicontact_api_geometry_second_order_cone_hpp__

#include <Eigen/Dense>
#include <iostream>

#include "multicontact-api/geometry/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"

namespace multicontact_api {
namespace geometry {

template <typename _Scalar, int _dim, int _Options>
struct SecondOrderCone : public serialization::Serializable<SecondOrderCone<_Scalar, _dim, _Options> > {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  enum { dim = _dim, Options = _Options };
  typedef _Scalar Scalar;
  typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
  typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
  typedef Eigen::DenseIndex DenseIndex;

  SecondOrderCone()
      : m_Q(MatrixD::Identity()), m_QPo(_dim, _dim), m_direction(VectorD::Zero()), m_Pd(_dim, _dim), m_Po(_dim, _dim) {
    m_direction[_dim - 1] = 1.;
    computeProjectors();
  }

  SecondOrderCone(const MatrixD& Q, const VectorD& direction)
      : m_Q(Q), m_QPo(_dim, _dim), m_direction(direction.normalized()), m_Pd(_dim, _dim), m_Po(_dim, _dim) {
    assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
    assert((Q - Q.transpose()).isMuchSmallerThan(Q));
    computeProjectors();
  }

  ///
  /// \brief Build a regular cone from a given friction coefficient and a direction.
  ///
  /// \param mu Friction coefficient.
  /// \param direction Direction of the cone.
  ///
  /// \returns A second order cone.
  ///
  static SecondOrderCone RegularCone(const Scalar mu, const VectorD& direction) {
    assert(mu > 0 && "The friction coefficient must be non-negative");
    MatrixD Q(MatrixD::Zero());
    Q.diagonal().fill(1. / mu);

    return SecondOrderCone(Q, direction);
  }

  template <typename S2, int O2>
  bool operator==(const SecondOrderCone<S2, dim, O2>& other) const {
    return m_Q == other.m_Q && m_direction == other.m_direction;
  }

  template <typename S2, int O2>
  bool operator!=(const SecondOrderCone<S2, dim, O2>& other) const {
    return !(*this == other);
  }

  /// \returns the value of lhs of the conic inequality
  Scalar lhsValue(const VectorD& point) const {
    //        const VectorD x_Po(m_Po * point);
    return (m_QPo * point).norm();
  }

  /// \returns the value of rhs of the conic inequality
  Scalar rhsValue(const VectorD& point) const { return m_direction.dot(point); }

  /// \returns true if the point is inside the cone
  bool check(const VectorD& point) const { return check(point, 1.); }

  bool check(const VectorD& point, const Scalar factor) const { return lhsValue(point) <= factor * rhsValue(point); }

  /// \returns the direction of the cone.
  const VectorD& direction() const { return m_direction; }
  void setDirection(const VectorD& direction) {
    assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
    m_direction = direction.normalized();
    computeProjectors();
  }

  template <typename S2, int O2>
  bool isApprox(const SecondOrderCone<S2, dim, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return m_direction.isApprox(other.m_direction, prec) && m_Q.isApprox(other.m_Q, prec);
  }

  /// \returns the quadratic term of the lhs norm.
  const MatrixD& Q() const { return m_Q; }
  void setQ(const MatrixD& Q) {
    assert((Q - Q.transpose()).isMuchSmallerThan(Q));
    m_Q = Q;
    computeProjectors();
  }

  void disp(std::ostream& os) const {
    os << "Q:\n" << m_Q << std::endl << "direction: " << m_direction.transpose() << std::endl;
  }

  friend std::ostream& operator<<(std::ostream& os, const SecondOrderCone& C) {
    C.disp(os);
    return os;
  }

 protected:
  inline void computeProjectors() {
    m_Pd = m_direction * m_direction.transpose();
    m_Po = MatrixD::Identity() - m_Pd;
    m_QPo.noalias() = m_Q * m_Po;
  }

  /// \brief Cholesky decomposition matrix reprensenting the conic norm
  MatrixD m_Q;
  /// \brief Cholesky decomposition projected on the orthogonal of m_direction
  MatrixD m_QPo;

  /// \brief Direction of the cone
  VectorD m_direction;

  /// \brief Projector along the direction of d
  MatrixD m_Pd;
  /// \brief Projector orthogonal to d
  MatrixD m_Po;

 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
    ar& boost::serialization::make_nvp("quadratic_term", m_Q);
    ar& boost::serialization::make_nvp("direction", m_direction);
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int /*version*/) {
    ar >> boost::serialization::make_nvp("quadratic_term", m_Q);
    ar >> boost::serialization::make_nvp("direction", m_direction);

    computeProjectors();
  }

  BOOST_SERIALIZATION_SPLIT_MEMBER()
};

}  // namespace geometry
}  // namespace multicontact_api

#endif  // ifndef __multicontact_api_geometry_second_order_cone_hpp__


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-arithmetic
    Function object implementing rec capabilities

    Returns the inverse (reciprocal) of the entry.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = rec(x);
    @endcode

    The code is similar to:

    @code
    T r = T(1)/x;
    @endcode

    @par Note

    For integral typed entries the result is always in the set \f$\{0,  \pm1, Valmax \}\f$

    @par Decorators

     For floating types @c rec has several decorated variations

     - raw_ Many simd architectures provide an intrinsic that computes some bits of the inverse (at least 12)
            and don't care of denormals or limiting values. If it exists this is obtained by the raw_ decorator.
            As usual if it doesn't the plain rec is called.

     - with no decorators ensure 1ulp but still doesn't care of limiting value or denormals

  **/
  Value rec(Value const & v0);
} }
#endif

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

#endif