#include <iostream>
#include <Eigen/Dense>
#include "Tools/Tools.h"
#include "ICP.h"

using namespace std;
using namespace Eigen;

// Matrix4d SVDsolve(const MatrixXd &pts1, const MatrixXd &pts2)
Matrix4d SVDsolve(const MatrixXd &pts1, const MatrixXd &pts2)
{
  Matrix4d T = MatrixXd::Identity(4,4);
  Vector3d p1(0,0,0);
  Vector3d p2(0,0,0);
  int N = pts1.rows();
  int m = pts1.cols();
  // cout<<"N:" << N << "   m:"<< m << endl;
  // cout<<"pts1:"<<pts1<< endl << "pts2:"<<pts2<<endl;
  MatrixXd q1(N, m);
  MatrixXd q2(N, m);

  for (int i=0; i<N; i++){
      p1 = p1 + pts1.block<1,3>(i,0).transpose();
      p2 = p2 + pts2.block<1,3>(i,0).transpose();
  };
  p1 = p1/N;
  p2 = p2/N;
  // cout<<"p1:"<< p1 <<endl<< "p2:"<< p2 << endl;

  for (int i=0; i<N; i++){
      q1.block<1, 3>(i, 0) = pts1.block<1, 3>(i, 0) - p1.transpose();  //(starting at)  <block of size>
      q2.block<1, 3>(i, 0) = pts2.block<1, 3>(i, 0) - p2.transpose();
  };
  // cout<<"q1:"<< q1 <<endl<< "q2:"<< q2 << endl;

  MatrixXd W = q1.transpose() * q2;
  // cout<<"W:"<<W<<endl;

  JacobiSVD<MatrixXd> svd(W, ComputeThinU | ComputeThinV);
    MatrixXd U = svd.matrixU();
    MatrixXd V = svd.matrixV();
    MatrixXd Vt = V.transpose();
  // cout<<"U:"<<U<<endl<<"V:"<<V<<endl<<"Vt:"<<Vt<<endl;

  MatrixXd R = Vt.transpose() * U.transpose();
  // cout<<"R:"<<R<<endl;

//   Matrix3d xx(3,3);
//   xx<<1,1,1,
//       1,1,1,
//       1,1,1 ;
//   xx.block<1,3>(2,0)*= -1;
//   cout<<xx<<endl;

  if(R.determinant() < 0){
 //R行列式<0
    Vt.block<1, 3>(2, 0) *= -1;       //第三行乘以-1
    R = Vt.transpose() * U.transpose();
    // cout<<"Vt:"<<Vt<<endl;
  };
  // cout<<"R:"<<R<<endl;
  Vector3d t = p2 - R*p1;      //(3,1) - (3,3)(3,1) = (3,1)
  // cout<<"t:"<<t<<endl;

  T.block<3,3>(0,0) = R ;
  T.block<3,1>(0,3) = t ;
  // cout<<"T"<<T<<endl;
  return T;
};

MatrixXd ICP(const MatrixXd pts1, const MatrixXd pts2, int max_iteration, float tolerance, float k ){
  int m = pts1.cols();
  int N1 = pts1.rows();
  int N2 = pts2.rows();
  MatrixXd src = MatrixXd::Ones(m+1, N1);    //(4, 12)
  MatrixXd dst = MatrixXd::Ones(m+1, N2);    //(4, 12)
  // cout<<"src:"<<src<<endl<<"dst:"<<dst<<endl;
  src.block(0,0,m,N1) = pts1.transpose();
  dst.block(0,0,m,N2) = pts2.transpose();
  // cout<<"src:"<<src<<endl<<"dst:"<<dst<<endl;

  float prev_error = 0;

  for(int i =0; i<max_iteration; i++){
    //   NEIGHBOR nearest_neighbot(src.block(0,0,m,N1), dst.block(0,0,m,N2));
    //   NEIGHBOR nighbor = nearest_neighbot(src.block(0,0,m,N1).transpose(), dst.block(0,0,m,N2).transpose());
      NEIGHBOR nighbor = nearest_neighbot(src.block(0,0,m,N1).transpose(), dst.block(0,0,m,N2).transpose());
      std::vector<float> distance = nighbor.distances;
      std::vector<int> indices = nighbor.indices;
      float mean_error=0;
      for(int i = 0; i<distance.size(); i++){
          mean_error = mean_error + distance[i];
          // cout<<"distance:"<<distance[i]<<endl;
      };
      mean_error = mean_error/distance.size();
      cout<<"mean_error:"<<mean_error<<endl;
      // for(int i = 0; i<indices.size(); i++){
      // cout<<"indices:"<<indices[i]<<endl;};

      int j = 0 ;
      for (int i = 0; i<N1 ; i++){
         if(distance[i]<=mean_error*k){
             j = j+1;
         };
      };
      cout<<"成功匹配:"<<j<<"对点"<<endl;

      MatrixXd pts11 = MatrixXd::Ones(j, 3);
      MatrixXd pts22 = MatrixXd::Ones(j, 3);
      // cout<<"pts11:"<<pts11<<endl<<"pts22:"<<pts22<<endl;

      // pts11.block<1,3>(0,0) = src.block<3,1>(0, 0);
      // pts11.block<1,3>(1,0) = src.block<3,1>(0, 1);

      int jj = 0;
      for (int i =0; i<N1; i++){
          if(distance[i]<= mean_error*k){
              pts11.block<1,3>(jj,0) = src.block<3,1>(0, i);
              pts22.block<1,3>(jj,0) = dst.block<3,1>(0, indices[i]);
              jj = jj +1;
                 }
          }
      cout<<"pts11:"<<pts11<<endl<<"pts22:"<<pts22<<endl;

      MatrixXd T = SVDsolve(pts11, pts22);
      cout << "T:" << T << endl;
      src = T*src;

      if (fabs(prev_error - mean_error) < tolerance){
        break;
      }
      prev_error = mean_error;

      }
  MatrixXd T = SVDsolve(pts1, src.block(0,0,m,N1).transpose());

  return T;

};


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include <Eigen/Dense>
#include <gtest/gtest.h>

using namespace Eigen;
using namespace std;

/** A macro for obtaining the name of the current function:  */
#if defined(_MSC_VER) && (_MSC_VER >= 1300)
#define __CURRENT_FUNCTION_NAME__ __FUNCTION__
#elif defined(_MSC_VER) && (_MSC_VER < 1300)
// Visual C++ 6 HAS NOT A __FUNCTION__ equivalent.
#define __CURRENT_FUNCTION_NAME__ ::system::extractFileName(__FILE__).c_str()
#else
#define __CURRENT_FUNCTION_NAME__ __PRETTY_FUNCTION__
#endif

#if 0

template <int ColRowOrder>
void do_test_EigenVal4x4_sym_vs_generic_eigen()
{
	using Mat44 = Matrix<double,4,4,ColRowOrder>;

	const double   dat_C1[] = {
		13.737245,10.248641,-5.839599,11.108320,
		10.248641,14.966139,-5.259922,11.662222,
		-5.839599,-5.259922,9.608822,-4.342505,
		11.108320,11.662222,-4.342505,12.121940 };
	const Mat44 C1(dat_C1);  // It doesn't mind the row/col major order since data are symetric

	// Symetric --------------------
	// This solver returns the eigenvectors already sorted.
	Eigen::SelfAdjointEigenSolver<Mat44> eigensolver(C1);
//	MatrixXd eVecs_s = eigensolver.eigenvectors();
//	MatrixXd eVals_s = eigensolver.eigenvalues();

	cout << endl << __CURRENT_FUNCTION_NAME__ << endl
		<< "SelfAdjointEigenSolver:\n"
		<< "eigvecs: " << endl << eigensolver.eigenvectors() << endl
		<< "eigvals: " << endl << eigensolver.eigenvalues() << endl;

	// Generic ---------------------
	Eigen::EigenSolver<Mat44> es(C1, true);
//	MatrixXd eVecs_g = es.eigenvectors().real();
//	MatrixXd eVals_g = es.eigenvalues().real();

	cout << endl
		<< "EigenSolver:\n"
		<< "eigvecs: " << endl << es.eigenvectors() << endl
		<< "eigvals: " << endl << es.eigenvalues() << endl;
}

// Compare the two ways of computing matrix eigenvectors: generic & for symmetric matrices:
TEST(MatricesEigen,EigenVal4x4_sym_vs_generic)
{
	do_test_EigenVal4x4_sym_vs_generic_eigen<Eigen::ColMajor>();
	do_test_EigenVal4x4_sym_vs_generic_eigen<Eigen::RowMajor>();
}

#endif


#pragma once

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

#include "numerical.hpp"

using namespace std;

/**
 * @brief 曲线基类，定义了必要的接口
 * @tparam N 曲线阶数
 * @tparam PointDim 数据点所在的维度
 */
template <int N = 3, int PointDim = 2>
class Curve
{
protected:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef Eigen::Matrix<double, PointDim, N+1> PointsType;

    double length_ {-1};

    /**
     * @brief 该方法计算 length_
     */
    virtual void computeLength()
    {
        auto df = [&](double t) -> double
        {
            return this->at(t,1).norm();
        };
        this->length_ = NumericalQuadrature::adaptive_simpson_3_8(df, 0, 1);
    }

    constexpr static const double EPS = 0.000001;

public:

    typedef Eigen::Matrix<double, PointDim, 1>  PointType;

    /**
     * @brief 返回该曲线的长度，而长度的信息应在 computeLength() 中计算完成
     * @return 派生类中曲线的长度
     */
    virtual const double& length() const {return length_;}

    /**
     * @brief 返回参数 $t$ 位置曲线上对应的点的位置，或$N$阶导数
     * @param t 参数位置，定义域为 $[0,1]$
     * @param derivative_order 求解的导数阶次，默认为$0$阶，即原函数值
     * @return $t$参数位置，$N$阶导数的值
     */
    virtual PointType at(const double& t, const int& derivative_order = 0) const = 0;

    /**
    * @brief 返回参数 $t$ 位置曲线上对应的点的位置，或$N$阶导数
    * @param t 参数位置，定义域为 $[0,1]$
    * @param derivative_order 求解的导数阶次，默认为$0$阶，即原函数值
    * @return $t$参数位置，$N$阶导数的值
    */
    virtual double findClosestParameter(const PointType& point, double init_param, const int& max_iter = 20) const
    {
        /**
         * 找到参数u使得离点p是最近的，即求解
         * f = (C(u)-p)*C'(u) = 0
         * f'= C'(u)*C'(u) + (C(u)-p)*C''(u)
         * 当给定u_{n+1} = u_{n} - f/f'
         */
        assert(max_iter>0);
        double numerator, denominator;
        for (int iter = 0; iter<max_iter; ++iter)
        {
            auto d = this->at(init_param) - point;
            auto first_order  = this->at(init_param, 1);
            auto second_order = this->at(init_param, 2);

            numerator   = d.dot(first_order);
            denominator = first_order.dot(first_order) + d.dot(second_order) + std::numeric_limits<double>::min();
            init_param = init_param - numerator/denominator;
        }

        return init_param;
    }

    /**
     * @brief 将曲线的参数内容传送到输出流对象中，默认必须末尾携带 `\n`
     * @param out 流对象，例如std::cout, std::stringsteam
     * @param s 设定的前缀
     */
    virtual void print(std::ostream& out, const std::string& s = "") const = 0;

    /**
     * @brief 近似地以$delta$间距均匀采集点
     * @param delta 参数间距或者距离间距，由$for_arc_length$指定
     * @param arc_length_t 返回最终得到的参数位置
     * @param for_arc_length 如果为`true`，则$delta$以长度为单位，否则以$t\in [0,1]$参数为单位
     * @param max_iter_time 最大迭代次数
     * @return 参数化采样点数据
     */
    virtual vector<PointType> sampleWithArcLengthParameterized(const double& delta, vector<double>& arc_length_t, bool arc_length_base = true,
                                                       const int& max_iter_time = 4)
    {
        const double avg_distance = arc_length_base? delta : this->length_ * delta;

        const double avg_t = arc_length_base? avg_distance/this->length_  : delta;

        assert( avg_distance >= 0 && avg_distance <= this->length_/2.0);

        const int n (this->length_ / avg_distance);

        vector<double> t_array(n, 0);
        vector<double>   dists(n, 0);
        vector<PointType> ret(n+1);       // 实际上总共有 n+1 个点
        ret.front() = this->at(0.0);
        ret.back() = this->at(1.0);

        // 第一个点与最后一个点保持在开始与结尾处
        for(int i = 1; i < n; ++i)
        {
            t_array[i] = i * avg_t;
            ret[i] = this->at(t_array[i]);
        }
        t_array.emplace_back(1.0);

        double prev_offset = -1;

        for(int iter = 0; iter < max_iter_time; ++iter)
        {
            // 1. 计算上一次迭代确定的 t 参数下，每一个分段的近似长度
            for (int j = 1; j < n; j++) dists[j] = (ret[j]-ret[j-1]).norm();

            double offset = 0;
            for (int j = 1; j < n; j++)
            {
                // 2. 累计近似弧长并计算误差
                offset += dists[j] - avg_distance;

                // 3. Newton's method
                double first_order  = this->at(t_array[j], 1).norm();
                double second_order = this->at(t_array[j], 2).norm();
                double numerator    = offset * first_order;
                double denominator  = offset * second_order + first_order * first_order;

                t_array[j] = t_array[j] - numerator / denominator;

                ret[j] = this->at(t_array[j]);
            }

            if ( offset < EPS || abs(offset-prev_offset) < EPS)
            {
                break;
            }
            prev_offset = offset;
        }

        arc_length_t.swap(t_array);

        return ret;
    }

    /**
     * @brief 在参数$t\in[0, 1]$取与弧长线性相关点
     * @param t 弧长参数化下的参数值，$t\in [0, 1]$
     * @param arc_length_t 返回最终计算得到的参数位置
     * @param derivative_order 给定的阶次, 默认为0阶
     * @param max_iter_time 最大迭代次数
     * @return 参数化点位置
     */
    virtual PointType atWithArcLengthParameterized(const double& t, double& arc_length_t,
        const int& derivative_order = 0, const int& max_iter_time = 4)  const
    {
        assert(t >= 0.0 && t <= 1.0);

        double approx_t = t, target_length = t * this->length_;
        double prev_approx_t = approx_t;

        const auto df = [&](double t) -> double{ return this->at(t, 1).norm(); };

        for(int iter = 0; iter < max_iter_time; ++iter)
        {
            double approx_length = NumericalQuadrature::adaptive_simpson_3_8(df, 0, approx_t);
            double d = approx_length - target_length;
            if (abs(d) < EPS) break;

            // Newton's method
            double first_order  = this->at(approx_t, 1).norm();
            double second_order = this->at(approx_t, 2).norm();
            double numerator    = d * first_order;
            double denominator  = d * second_order + first_order * first_order;

            approx_t = approx_t - numerator / denominator;

            if ( abs(approx_t-prev_approx_t) < EPS) break;
            else prev_approx_t = approx_t;
        }
        arc_length_t = approx_t;
        return this->at(approx_t, derivative_order);
    }

};


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

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

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

#ifndef _PIMATH_LINEALGO__H_
#define _PIMATH_LINEALGO__H_

#include <boost/python.hpp>
#include <ImathVecAlgo.h>
#include <ImathVec.h>
#include "util.h"

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct VecAlgoBind
	{
		VecAlgoBind()
		{
			bp::def("project", Imath::project<T>);
			bp::def("orthogonal", Imath::orthogonal<T>);
			bp::def("reflect", Imath::reflect<T> );
			bp::def("closestVertex", Imath::closestVertex<T> );
		}
	};
}

#endif


//  Copyright (c) 2000-2011 Joerg Walter, Mathias Koch, David Bellot
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.

#ifndef _BOOST_UBLAS_BLAS_
#define _BOOST_UBLAS_BLAS_

#include <boost/numeric/ublas/traits.hpp>

namespace boost { namespace numeric { namespace ublas {

    /** Interface and implementation of BLAS level 1
     * This includes functions which perform \b vector-vector operations.
     * More information about BLAS can be found at
     * http://en.wikipedia.org/wiki/BLAS
     */
    namespace blas_1 {

        /** 1-Norm: \f$\sum_i |x_i|\f$ (also called \f$\mathcal{L}_1\f$ or Manhattan norm)
     *
     * \param v a vector or vector expression
     * \return the 1-Norm with type of the vector's type
     *
     * \tparam V type of the vector (not needed by default)
     */
        template<class V>
        typename type_traits<typename V::value_type>::real_type
        asum (const V &v) {
            return norm_1 (v);
        }

        /** 2-Norm: \f$\sum_i |x_i|^2\f$ (also called \f$\mathcal{L}_2\f$ or Euclidean norm)
     *
     * \param v a vector or vector expression
     * \return the 2-Norm with type of the vector's type
     *
     * \tparam V type of the vector (not needed by default)
     */
        template<class V>
        typename type_traits<typename V::value_type>::real_type
        nrm2 (const V &v) {
            return norm_2 (v);
        }

        /** Infinite-norm: \f$\max_i |x_i|\f$ (also called \f$\mathcal{L}_\infty\f$ norm)
     *
     * \param v a vector or vector expression
     * \return the Infinite-Norm with type of the vector's type
     *
     * \tparam V type of the vector (not needed by default)
     */
        template<class V>
        typename type_traits<typename V::value_type>::real_type
        amax (const V &v) {
            return norm_inf (v);
        }

        /** Inner product of vectors \f$v_1\f$ and \f$v_2\f$
     *
     * \param v1 first vector of the inner product
     * \param v2 second vector of the inner product
     * \return the inner product of the type of the most generic type of the 2 vectors
     *
     * \tparam V1 type of first vector (not needed by default)
     * \tparam V2 type of second vector (not needed by default)
     */
        template<class V1, class V2>
        typename promote_traits<typename V1::value_type, typename V2::value_type>::promote_type
        dot (const V1 &v1, const V2 &v2) {
            return inner_prod (v1, v2);
        }

        /** Copy vector \f$v_2\f$ to \f$v_1\f$
     *
     * \param v1 target vector
     * \param v2 source vector
     * \return a reference to the target vector
     *
     * \tparam V1 type of first vector (not needed by default)
     * \tparam V2 type of second vector (not needed by default)
     */
        template<class V1, class V2>
        V1 & copy (V1 &v1, const V2 &v2)
    {
            return v1.assign (v2);
        }

        /** Swap vectors \f$v_1\f$ and \f$v_2\f$
     *
     * \param v1 first vector
     * \param v2 second vector
     *
         * \tparam V1 type of first vector (not needed by default)
     * \tparam V2 type of second vector (not needed by default)
     */
    template<class V1, class V2>
        void swap (V1 &v1, V2 &v2)
    {
            v1.swap (v2);
        }

        /** scale vector \f$v\f$ with scalar \f$t\f$
     *
     * \param v vector to be scaled
     * \param t the scalar
     * \return \c t*v
     *
     * \tparam V type of the vector (not needed by default)
     * \tparam T type of the scalar (not needed by default)
     */
        template<class V, class T>
        V & scal (V &v, const T &t)
    {
            return v *= t;
        }

        /** Compute \f$v_1= v_1 +  t.v_2\f$
     *
     * \param v1 target and first vector
     * \param t the scalar
     * \param v2 second vector
     * \return a reference to the first and target vector
     *
     * \tparam V1 type of the first vector (not needed by default)
     * \tparam T type of the scalar (not needed by default)
     * \tparam V2 type of the second vector (not needed by default)
     */
        template<class V1, class T, class V2>
        V1 & axpy (V1 &v1, const T &t, const V2 &v2)
    {
            return v1.plus_assign (t * v2);
        }

    /** Performs rotation of points in the plane and assign the result to the first vector
     *
     * Each point is defined as a pair \c v1(i) and \c v2(i), being respectively
     * the \f$x\f$ and \f$y\f$ coordinates. The parameters \c t1 and \t2 are respectively
     * the cosine and sine of the angle of the rotation.
     * Results are not returned but directly written into \c v1.
     *
     * \param t1 cosine of the rotation
     * \param v1 vector of \f$x\f$ values
     * \param t2 sine of the rotation
     * \param v2 vector of \f$y\f$ values
     *
     * \tparam T1 type of the cosine value (not needed by default)
     * \tparam V1 type of the \f$x\f$ vector (not needed by default)
     * \tparam T2 type of the sine value (not needed by default)
     * \tparam V2 type of the \f$y\f$ vector (not needed by default)
     */
        template<class T1, class V1, class T2, class V2>
        void rot (const T1 &t1, V1 &v1, const T2 &t2, V2 &v2)
    {
            typedef typename promote_traits<typename V1::value_type, typename V2::value_type>::promote_type promote_type;
            vector<promote_type> vt (t1 * v1 + t2 * v2);
            v2.assign (- t2 * v1 + t1 * v2);
            v1.assign (vt);
        }

    }

    /** \brief Interface and implementation of BLAS level 2
     * This includes functions which perform \b matrix-vector operations.
     * More information about BLAS can be found at
     * http://en.wikipedia.org/wiki/BLAS
     */
    namespace blas_2 {

       /** \brief multiply vector \c v with triangular matrix \c m
    *
    * \param v a vector
    * \param m a triangular matrix
    * \return the result of the product
    *
    * \tparam V type of the vector (not needed by default)
    * \tparam M type of the matrix (not needed by default)
        */
        template<class V, class M>
        V & tmv (V &v, const M &m)
    {
            return v = prod (m, v);
        }

        /** \brief solve \f$m.x = v\f$ in place, where \c m is a triangular matrix
     *
     * \param v a vector
     * \param m a matrix
     * \param C (this parameter is not needed)
     * \return a result vector from the above operation
     *
     * \tparam V type of the vector (not needed by default)
     * \tparam M type of the matrix (not needed by default)
     * \tparam C n/a
         */
        template<class V, class M, class C>
        V & tsv (V &v, const M &m, C)
    {
            return v = solve (m, v, C ());
        }

        /** \brief compute \f$ v_1 = t_1.v_1 + t_2.(m.v_2)\f$, a general matrix-vector product
     *
     * \param v1 a vector
     * \param t1 a scalar
     * \param t2 another scalar
     * \param m a matrix
     * \param v2 another vector
     * \return the vector \c v1 with the result from the above operation
     *
     * \tparam V1 type of first vector (not needed by default)
     * \tparam T1 type of first scalar (not needed by default)
     * \tparam T2 type of second scalar (not needed by default)
     * \tparam M type of matrix (not needed by default)
     * \tparam V2 type of second vector (not needed by default)
         */
        template<class V1, class T1, class T2, class M, class V2>
        V1 & gmv (V1 &v1, const T1 &t1, const T2 &t2, const M &m, const V2 &v2)
    {
            return v1 = t1 * v1 + t2 * prod (m, v2);
        }

        /** \brief Rank 1 update: \f$ m = m + t.(v_1.v_2^T)\f$
     *
     * \param m a matrix
     * \param t a scalar
     * \param v1 a vector
     * \param v2 another vector
     * \return a matrix with the result from the above operation
     *
     * \tparam M type of matrix (not needed by default)
     * \tparam T type of scalar (not needed by default)
     * \tparam V1 type of first vector (not needed by default)
     * \tparam V2type of second vector (not needed by default)
     */
        template<class M, class T, class V1, class V2>
        M & gr (M &m, const T &t, const V1 &v1, const V2 &v2)
    {
#ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG
            return m += t * outer_prod (v1, v2);
#else
            return m = m + t * outer_prod (v1, v2);
#endif
        }

        /** \brief symmetric rank 1 update: \f$m = m + t.(v.v^T)\f$
     *
     * \param m a matrix
     * \param t a scalar
     * \param v a vector
     * \return a matrix with the result from the above operation
     *
     * \tparam M type of matrix (not needed by default)
     * \tparam T type of scalar (not needed by default)
     * \tparam V type of vector (not needed by default)
     */
        template<class M, class T, class V>
        M & sr (M &m, const T &t, const V &v)
    {
#ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG
            return m += t * outer_prod (v, v);
#else
            return m = m + t * outer_prod (v, v);
#endif
        }

        /** \brief hermitian rank 1 update: \f$m = m + t.(v.v^H)\f$
     *
     * \param m a matrix
     * \param t a scalar
     * \param v a vector
     * \return a matrix with the result from the above operation
     *
     * \tparam M type of matrix (not needed by default)
     * \tparam T type of scalar (not needed by default)
     * \tparam V type of vector (not needed by default)
     */
        template<class M, class T, class V>
        M & hr (M &m, const T &t, const V &v)
    {
#ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG
            return m += t * outer_prod (v, conj (v));
#else
            return m = m + t * outer_prod (v, conj (v));
#endif
        }

         /** \brief symmetric rank 2 update: \f$ m=m+ t.(v_1.v_2^T + v_2.v_1^T)\f$
      *
      * \param m a matrix
      * \param t a scalar
      * \param v1 a vector
      * \param v2 another vector
      * \return a matrix with the result from the above operation
      *
      * \tparam M type of matrix (not needed by default)
      * \tparam T type of scalar (not needed by default)
      * \tparam V1 type of first vector (not needed by default)
      * \tparam V2type of second vector (not needed by default)
          */
        template<class M, class T, class V1, class V2>
        M & sr2 (M &m, const T &t, const V1 &v1, const V2 &v2)
    {
#ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG
            return m += t * (outer_prod (v1, v2) + outer_prod (v2, v1));
#else
            return m = m + t * (outer_prod (v1, v2) + outer_prod (v2, v1));
#endif
        }

        /** \brief hermitian rank 2 update: \f$m=m+t.(v_1.v_2^H) + v_2.(t.v_1)^H)\f$
     *
     * \param m a matrix
     * \param t a scalar
     * \param v1 a vector
     * \param v2 another vector
     * \return a matrix with the result from the above operation
     *
     * \tparam M type of matrix (not needed by default)
     * \tparam T type of scalar (not needed by default)
     * \tparam V1 type of first vector (not needed by default)
     * \tparam V2type of second vector (not needed by default)
         */
        template<class M, class T, class V1, class V2>
        M & hr2 (M &m, const T &t, const V1 &v1, const V2 &v2)
    {
#ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG
            return m += t * outer_prod (v1, conj (v2)) + type_traits<T>::conj (t) * outer_prod (v2, conj (v1));
#else
            return m = m + t * outer_prod (v1, conj (v2)) + type_traits<T>::conj (t) * outer_prod (v2, conj (v1));
#endif
        }

    }

    /** \brief Interface and implementation of BLAS level 3
     * This includes functions which perform \b matrix-matrix operations.
     * More information about BLAS can be found at
     * http://en.wikipedia.org/wiki/BLAS
     */
    namespace blas_3 {

        /** \brief triangular matrix multiplication \f$m_1=t.m_2.m_3\f$ where \f$m_2\f$ and \f$m_3\f$ are triangular
     *
     * \param m1 a matrix for storing result
     * \param t a scalar
     * \param m2 a triangular matrix
     * \param m3 a triangular matrix
     * \return the matrix \c m1
     *
     * \tparam M1 type of the result matrix (not needed by default)
     * \tparam T type of the scalar (not needed by default)
     * \tparam M2 type of the first triangular matrix (not needed by default)
     * \tparam M3 type of the second triangular matrix (not needed by default)
     *
        */
        template<class M1, class T, class M2, class M3>
        M1 & tmm (M1 &m1, const T &t, const M2 &m2, const M3 &m3)
    {
            return m1 = t * prod (m2, m3);
        }

        /** \brief triangular solve \f$ m_2.x = t.m_1\f$ in place, \f$m_2\f$ is a triangular matrix
     *
     * \param m1 a matrix
     * \param t a scalar
     * \param m2 a triangular matrix
     * \param C (not used)
     * \return the \f$m_1\f$ matrix
     *
     * \tparam M1 type of the first matrix (not needed by default)
     * \tparam T type of the scalar (not needed by default)
     * \tparam M2 type of the triangular matrix (not needed by default)
     * \tparam C (n/a)
         */
        template<class M1, class T, class M2, class C>
        M1 & tsm (M1 &m1, const T &t, const M2 &m2, C)
    {
            return m1 = solve (m2, t * m1, C ());
        }

        /** \brief general matrix multiplication \f$m_1=t_1.m_1 + t_2.m_2.m_3\f$
     *
     * \param m1 first matrix
     * \param t1 first scalar
     * \param t2 second scalar
     * \param m2 second matrix
     * \param m3 third matrix
     * \return the matrix \c m1
     *
     * \tparam M1 type of the first matrix (not needed by default)
     * \tparam T1 type of the first scalar (not needed by default)
     * \tparam T2 type of the second scalar (not needed by default)
     * \tparam M2 type of the second matrix (not needed by default)
     * \tparam M3 type of the third matrix (not needed by default)
         */
        template<class M1, class T1, class T2, class M2, class M3>
        M1 & gmm (M1 &m1, const T1 &t1, const T2 &t2, const M2 &m2, const M3 &m3)
    {
            return m1 = t1 * m1 + t2 * prod (m2, m3);
        }

        /** \brief symmetric rank \a k update: \f$m_1=t.m_1+t_2.(m_2.m_2^T)\f$
     *
     * \param m1 first matrix
     * \param t1 first scalar
     * \param t2 second scalar
     * \param m2 second matrix
     * \return matrix \c m1
     *
     * \tparam M1 type of the first matrix (not needed by default)
     * \tparam T1 type of the first scalar (not needed by default)
     * \tparam T2 type of the second scalar (not needed by default)
     * \tparam M2 type of the second matrix (not needed by default)
     * \todo use opb_prod()
         */
        template<class M1, class T1, class T2, class M2>
        M1 & srk (M1 &m1, const T1 &t1, const T2 &t2, const M2 &m2)
    {
            return m1 = t1 * m1 + t2 * prod (m2, trans (m2));
        }

        /** \brief hermitian rank \a k update: \f$m_1=t.m_1+t_2.(m_2.m2^H)\f$
     *
     * \param m1 first matrix
     * \param t1 first scalar
     * \param t2 second scalar
     * \param m2 second matrix
     * \return matrix \c m1
     *
     * \tparam M1 type of the first matrix (not needed by default)
     * \tparam T1 type of the first scalar (not needed by default)
     * \tparam T2 type of the second scalar (not needed by default)
     * \tparam M2 type of the second matrix (not needed by default)
     * \todo use opb_prod()
         */
        template<class M1, class T1, class T2, class M2>
        M1 & hrk (M1 &m1, const T1 &t1, const T2 &t2, const M2 &m2)
    {
            return m1 = t1 * m1 + t2 * prod (m2, herm (m2));
        }

        /** \brief generalized symmetric rank \a k update: \f$m_1=t_1.m_1+t_2.(m_2.m3^T)+t_2.(m_3.m2^T)\f$
     *
     * \param m1 first matrix
     * \param t1 first scalar
     * \param t2 second scalar
     * \param m2 second matrix
     * \param m3 third matrix
     * \return matrix \c m1
     *
     * \tparam M1 type of the first matrix (not needed by default)
     * \tparam T1 type of the first scalar (not needed by default)
     * \tparam T2 type of the second scalar (not needed by default)
     * \tparam M2 type of the second matrix (not needed by default)
     * \tparam M3 type of the third matrix (not needed by default)
     * \todo use opb_prod()
         */
        template<class M1, class T1, class T2, class M2, class M3>
        M1 & sr2k (M1 &m1, const T1 &t1, const T2 &t2, const M2 &m2, const M3 &m3)
    {
            return m1 = t1 * m1 + t2 * (prod (m2, trans (m3)) + prod (m3, trans (m2)));
        }

        /** \brief generalized hermitian rank \a k update: * \f$m_1=t_1.m_1+t_2.(m_2.m_3^H)+(m_3.(t_2.m_2)^H)\f$
     *
     * \param m1 first matrix
     * \param t1 first scalar
     * \param t2 second scalar
     * \param m2 second matrix
     * \param m3 third matrix
     * \return matrix \c m1
     *
     * \tparam M1 type of the first matrix (not needed by default)
     * \tparam T1 type of the first scalar (not needed by default)
     * \tparam T2 type of the second scalar (not needed by default)
     * \tparam M2 type of the second matrix (not needed by default)
     * \tparam M3 type of the third matrix (not needed by default)
     * \todo use opb_prod()
         */
        template<class M1, class T1, class T2, class M2, class M3>
        M1 & hr2k (M1 &m1, const T1 &t1, const T2 &t2, const M2 &m2, const M3 &m3)
    {
            return m1 =
              t1 * m1
            + t2 * prod (m2, herm (m3))
            + type_traits<T2>::conj (t2) * prod (m3, herm (m2));
        }

    }

}}}

#endif


//
// Copyright (c) 2012 Ronaldo Carpio
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  The authors make no representations
// about the suitability of this software for any purpose.
// It is provided "as is" without express or implied warranty.
//

/*
This is a C++ header-only library for N-dimensional linear interpolation on a
rectangular grid. Implements two methods:
* Multilinear: Interpolate using the N-dimensional hypercube containing the
point. Interpolation step is O(2^N)
* Simplicial: Interpolate using the N-dimensional simplex containing the point.
Interpolation step is O(N log N), but less accurate. Requires boost/multi_array
library.

For a description of the algorithms, see:
* Weiser & Zarantonello (1988), "A Note on Piecewise Linear and Multilinear
Table Interpolation in Many Dimensions", _Mathematics of Computation_ 50 (181),
p. 189-196
* Davies (1996), "Multidimensional Triangulation and Interpolation for
Reinforcement Learning", _Proceedings of Neural Information Processing Systems
1996_
*/

#ifndef _linterp_h
#define _linterp_h

#include <array>
#include <assert.h>
#include <cstdarg>
#include <float.h>
#include <functional>
#include <math.h>
#include <stdarg.h>
#include <string>
#include <vector>

#include <boost/multi_array.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/storage.hpp>

using std::array;
using std::vector;
typedef unsigned int uint;
typedef vector<int> iVec;
typedef vector<double> dVec;

// TODO:
//  - specify behavior past grid boundaries.
//    1) clamp
//    2) return a pre-determined value (e.g. NaN)

// compile-time params:
//   1) number of dimensions
//   2) scalar type T
//   3) copy data or not (default: false). The grids will always be copied
//   4) ref count class (default: none)
//   5) continuous or not

// run-time constructor params:
//   1) f
//   2) grids
//   3) behavior outside grid: default=clamp
//   4) value to return outside grid, defaut=nan

struct EmptyClass {};

template <int N, class T, bool CopyData = true, bool Continuous = true,
          class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
class NDInterpolator {
public:
  typedef T value_type;
  typedef ArrayRefCountT array_ref_count_type;
  typedef GridRefCountT grid_ref_count_type;

  static const int m_N = N;
  static const bool m_bCopyData = CopyData;
  static const bool m_bContinuous = Continuous;

  typedef boost::numeric::ublas::array_adaptor<T> grid_type;
  typedef boost::const_multi_array_ref<T, N> array_type;
  typedef std::unique_ptr<array_type> array_type_ptr;

  array_type_ptr m_pF;
  ArrayRefCountT m_ref_F; // reference count for m_pF
  vector<T> m_F_copy;     // if CopyData == true, this holds the copy of F

  vector<grid_type> m_grid_list;
  vector<GridRefCountT> m_grid_ref_list; // reference counts for grids
  vector<vector<T>> m_grid_copy_list;    // if CopyData == true, this holds the
                                         // copies of the grids

  // constructors assume that [f_begin, f_end) is a contiguous array in C-order
  // non ref-counted constructor.
  template <class IterT1, class IterT2, class IterT3>
  NDInterpolator(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                 IterT3 f_end) {
    init(grids_begin, grids_len_begin, f_begin, f_end);
  }

  // ref-counted constructor
  template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
  NDInterpolator(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                 IterT3 f_end, ArrayRefCountT &refF,
                 RefCountIterT grid_refs_begin) {
    init_refcount(grids_begin, grids_len_begin, f_begin, f_end, refF,
                  grid_refs_begin);
  }

  template <class IterT1, class IterT2, class IterT3>
  void init(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
            IterT3 f_end) {
    set_grids(grids_begin, grids_len_begin, m_bCopyData);
    set_f_array(f_begin, f_end, m_bCopyData);
  }
  template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
  void init_refcount(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                     IterT3 f_end, ArrayRefCountT &refF,
                     RefCountIterT grid_refs_begin) {
    set_grids(grids_begin, grids_len_begin, m_bCopyData);
    set_grids_refcount(grid_refs_begin, grid_refs_begin + N);
    set_f_array(f_begin, f_end, m_bCopyData);
    set_f_refcount(refF);
  }

  template <class IterT1, class IterT2>
  void set_grids(IterT1 grids_begin, IterT2 grids_len_begin, bool bCopy) {
    m_grid_list.clear();
    m_grid_ref_list.clear();
    m_grid_copy_list.clear();
    for (int i = 0; i < N; i++) {
      int gridLength = grids_len_begin[i];
      if (bCopy == false) {
        T const *grid_ptr = &(*grids_begin[i]);
        m_grid_list.push_back(
            grid_type(gridLength, (T *)grid_ptr)); // use the given pointer
      } else {
        m_grid_copy_list.push_back(
            vector<T>(grids_begin[i],
                      grids_begin[i] +
                          grids_len_begin[i])); // make our own copy of the grid
        T *begin = &(m_grid_copy_list[i][0]);
        m_grid_list.push_back(grid_type(gridLength, begin)); // use our copy
      }
    }
  }
  template <class IterT1, class RefCountIterT>
  void set_grids_refcount(RefCountIterT refs_begin, RefCountIterT refs_end) {
    assert(refs_end - refs_begin == N);
    m_grid_ref_list.assign(refs_begin, refs_begin + N);
  }

  // assumes that [f_begin, f_end) is a contiguous array in C-order
  template <class IterT>
  void set_f_array(IterT f_begin, IterT f_end, bool bCopy) {
    unsigned int nGridPoints = 1;
    array<int, N> sizes;
    for (unsigned int i = 0; i < m_grid_list.size(); i++) {
      sizes[i] = m_grid_list[i].size();
      nGridPoints *= sizes[i];
    }

    int f_len = f_end - f_begin;
    if ((m_bContinuous && f_len != nGridPoints) ||
        (!m_bContinuous && f_len != 2 * nGridPoints)) {
      throw std::invalid_argument("f has wrong size");
    }
    for (unsigned int i = 0; i < m_grid_list.size(); i++) {
      if (!m_bContinuous) {
        sizes[i] *= 2;
      }
    }

    m_F_copy.clear();
    if (bCopy == false) {
      m_pF.reset(new array_type(f_begin, sizes));
    } else {
      m_F_copy = vector<T>(f_begin, f_end);
      m_pF.reset(new array_type(&m_F_copy[0], sizes));
    }
  }
  void set_f_refcount(ArrayRefCountT &refF) { m_ref_F = refF; }

  // -1 is before the first grid point
  // N-1 (where grid.size() == N) is after the last grid point
  int find_cell(int dim, T x) const {
    grid_type const &grid(m_grid_list[dim]);
    if (x < *(grid.begin()))
      return -1;
    else if (x >= *(grid.end() - 1))
      return grid.size() - 1;
    else {
      auto i_upper = std::upper_bound(grid.begin(), grid.end(), x);
      return i_upper - grid.begin() - 1;
    }
  }

  // return the value of f at the given cell and vertex
  T get_f_val(array<int, N> const &cell_index,
              array<int, N> const &v_index) const {
    array<int, N> f_index;

    if (m_bContinuous) {
      for (int i = 0; i < N; i++) {
        if (cell_index[i] < 0) {
          f_index[i] = 0;
        } else if (cell_index[i] >= m_grid_list[i].size() - 1) {
          f_index[i] = m_grid_list[i].size() - 1;
        } else {
          f_index[i] = cell_index[i] + v_index[i];
        }
      }
    } else {
      for (int i = 0; i < N; i++) {
        if (cell_index[i] < 0) {
          f_index[i] = 0;
        } else if (cell_index[i] >= m_grid_list[i].size() - 1) {
          f_index[i] = (2 * m_grid_list[i].size()) - 1;
        } else {
          f_index[i] = 1 + (2 * cell_index[i]) + v_index[i];
        }
      }
    }
    return (*m_pF)(f_index);
  }

  T get_f_val(array<int, N> const &cell_index, int v) const {
    array<int, N> v_index;
    for (int dim = 0; dim < N; dim++) {
      v_index[dim] = (v >> (N - dim - 1)) & 1; // test if the i-th bit is set
    }
    return get_f_val(cell_index, v_index);
  }
};

template <int N, class T, bool CopyData = true, bool Continuous = true,
          class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
class InterpSimplex : public NDInterpolator<N, T, CopyData, Continuous,
                                            ArrayRefCountT, GridRefCountT> {
public:
  typedef NDInterpolator<N, T, CopyData, Continuous, ArrayRefCountT,
                         GridRefCountT>
      super;

  template <class IterT1, class IterT2, class IterT3>
  InterpSimplex(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                IterT3 f_end)
      : super(grids_begin, grids_len_begin, f_begin, f_end) {}
  template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
  InterpSimplex(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                IterT3 f_end, ArrayRefCountT &refF, RefCountIterT ref_begins)
      : super(grids_begin, grids_len_begin, f_begin, f_end, refF, ref_begins) {}

  template <class IterT> T interp(IterT x_begin) const {
    array<T, 1> result;
    array<array<T, 1>, N> coord_iter;
    for (int i = 0; i < N; i++) {
      coord_iter[i][0] = x_begin[i];
    }
    interp_vec(1, coord_iter.begin(), coord_iter.end(), result.begin());
    return result[0];
  }

  template <class IterT1, class IterT2>
  void interp_vec(int n, IterT1 coord_iter_begin, IterT1 coord_iter_end,
                  IterT2 i_result) const {
    assert(N == coord_iter_end - coord_iter_begin);

    array<int, N> cell_index, v_index;
    array<std::pair<T, int>, N> xipair;
    int c;
    T y, v0, v1;
    // mexPrintf("%d\n", n);
    for (int i = 0; i < n; i++) { // for each point
      for (int dim = 0; dim < N; dim++) {
        typename super::grid_type const &grid(super::m_grid_list[dim]);
        c = this->find_cell(dim, coord_iter_begin[dim][i]);
        // mexPrintf("%d\n", c);
        if (c == -1) { // before first grid point
          y = 1.0;
        } else if (c == grid.size() - 1) { // after last grid point
          y = 0.0;
        } else {
          // mexPrintf("%f %f\n", grid[c], grid[c+1]);
          y = (coord_iter_begin[dim][i] - grid[c]) / (grid[c + 1] - grid[c]);
          if (y < 0.0)
            y = 0.0;
          else if (y > 1.0)
            y = 1.0;
        }
        xipair[dim].first = y;
        xipair[dim].second = dim;
        cell_index[dim] = c;
      }
      // sort xi's and get the permutation
      std::sort(xipair.begin(), xipair.end(),
                [](std::pair<T, int> const &a, std::pair<T, int> const &b) {
                  return (a.first < b.first);
                });
      // walk the vertices of the simplex determined by the permutation
      for (int j = 0; j < N; j++) {
        v_index[j] = 1;
      }
      v0 = this->get_f_val(cell_index, v_index);
      y = v0;
      for (int j = 0; j < N; j++) {
        v_index[xipair[j].second]--;
        v1 = this->get_f_val(cell_index, v_index);
        y += (1.0 - xipair[j].first) * (v1 - v0); // interpolate
        v0 = v1;
      }
      *i_result++ = y;
    }
  }
};

template <int N, class T, bool CopyData = true, bool Continuous = true,
          class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
class InterpMultilinear : public NDInterpolator<N, T, CopyData, Continuous,
                                                ArrayRefCountT, GridRefCountT> {
public:
  typedef NDInterpolator<N, T, CopyData, Continuous, ArrayRefCountT,
                         GridRefCountT>
      super;

  template <class IterT1, class IterT2, class IterT3>
  InterpMultilinear(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                    IterT3 f_end)
      : super(grids_begin, grids_len_begin, f_begin, f_end) {}
  template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
  InterpMultilinear(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin,
                    IterT3 f_end, ArrayRefCountT &refF,
                    RefCountIterT ref_begins)
      : super(grids_begin, grids_len_begin, f_begin, f_end, refF, ref_begins) {}

  template <class IterT1, class IterT2>
  static T linterp_nd_unitcube(IterT1 f_begin, IterT1 f_end, IterT2 xi_begin,
                               IterT2 xi_end) {
    int n = xi_end - xi_begin;
    int f_len = f_end - f_begin;
    assert(1 << n == f_len);
    T sub_lower, sub_upper;
    if (n == 1) {
      sub_lower = f_begin[0];
      sub_upper = f_begin[1];
    } else {
      sub_lower = linterp_nd_unitcube(f_begin, f_begin + (f_len / 2),
                                      xi_begin + 1, xi_end);
      sub_upper = linterp_nd_unitcube(f_begin + (f_len / 2), f_end,
                                      xi_begin + 1, xi_end);
    }
    T result = sub_lower + (*xi_begin) * (sub_upper - sub_lower);
    return result;
  }

  template <class IterT> T interp(IterT x_begin) const {
    array<T, 1> result;
    array<array<T, 1>, N> coord_iter;
    for (int i = 0; i < N; i++) {
      coord_iter[i][0] = x_begin[i];
    }
    interp_vec(1, coord_iter.begin(), coord_iter.end(), result.begin());
    return result[0];
  }

  template <class IterT1, class IterT2>
  void interp_vec(int n, IterT1 coord_iter_begin, IterT1 coord_iter_end,
                  IterT2 i_result) const {
    assert(N == coord_iter_end - coord_iter_begin);
    array<int, N> index;
    int c;
    T y, xi;
    vector<T> f(1 << N);
    array<T, N> x;

    for (int i = 0; i < n; i++) {         // loop over each point
      for (int dim = 0; dim < N; dim++) { // loop over each dimension
        auto const &grid(super::m_grid_list[dim]);
        xi = coord_iter_begin[dim][i];
        c = this->find_cell(dim, coord_iter_begin[dim][i]);
        if (c == -1) { // before first grid point
          y = 1.0;
        } else if (c == grid.size() - 1) { // after last grid point
          y = 0.0;
        } else {
          y = (coord_iter_begin[dim][i] - grid[c]) / (grid[c + 1] - grid[c]);
          if (y < 0.0)
            y = 0.0;
          else if (y > 1.0)
            y = 1.0;
        }
        index[dim] = c;
        x[dim] = y;
      }
      // copy f values at vertices
      for (int v = 0; v < (1 << N); v++) { // loop over each vertex of hypercube
        f[v] = this->get_f_val(index, v);
      }
      *i_result++ = linterp_nd_unitcube(f.begin(), f.end(), x.begin(), x.end());
    }
  }
};

typedef InterpSimplex<1, double> NDInterpolator_1_S;
typedef InterpSimplex<2, double> NDInterpolator_2_S;
typedef InterpSimplex<3, double> NDInterpolator_3_S;
typedef InterpSimplex<4, double> NDInterpolator_4_S;
typedef InterpSimplex<5, double> NDInterpolator_5_S;
typedef InterpMultilinear<1, double> NDInterpolator_1_ML;
typedef InterpMultilinear<2, double> NDInterpolator_2_ML;
typedef InterpMultilinear<3, double> NDInterpolator_3_ML;
typedef InterpMultilinear<4, double> NDInterpolator_4_ML;
typedef InterpMultilinear<5, double> NDInterpolator_5_ML;

// C interface
extern "C" {
void linterp_simplex_1(double **grids_begin, int *grid_len_begin, double *pF,
                       int xi_len, double **xi_begin, double *pResult);
void linterp_simplex_2(double **grids_begin, int *grid_len_begin, double *pF,
                       int xi_len, double **xi_begin, double *pResult);
void linterp_simplex_3(double **grids_begin, int *grid_len_begin, double *pF,
                       int xi_len, double **xi_begin, double *pResult);
}

void linterp_simplex_1(double **grids_begin, int *grid_len_begin, double *pF,
                       int xi_len, double **xi_begin, double *pResult) {
  const int N = 1;
  size_t total_size = 1;
  for (int i = 0; i < N; i++) {
    total_size *= grid_len_begin[i];
  }
  InterpSimplex<N, double, false> interp_obj(grids_begin, grid_len_begin, pF,
                                             pF + total_size);
  interp_obj.interp_vec(xi_len, xi_begin, xi_begin + N, pResult);
}

void linterp_simplex_2(double **grids_begin, int *grid_len_begin, double *pF,
                       int xi_len, double **xi_begin, double *pResult) {
  const int N = 2;
  size_t total_size = 1;
  for (int i = 0; i < N; i++) {
    total_size *= grid_len_begin[i];
  }
  InterpSimplex<N, double, false> interp_obj(grids_begin, grid_len_begin, pF,
                                             pF + total_size);
  interp_obj.interp_vec(xi_len, xi_begin, xi_begin + N, pResult);
}

void linterp_simplex_3(double **grids_begin, int *grid_len_begin, double *pF,
                       int xi_len, double **xi_begin, double *pResult) {
  const int N = 3;
  size_t total_size = 1;
  for (int i = 0; i < N; i++) {
    total_size *= grid_len_begin[i];
  }
  InterpSimplex<N, double, false> interp_obj(grids_begin, grid_len_begin, pF,
                                             pF + total_size);
  interp_obj.interp_vec(xi_len, xi_begin, xi_begin + N, pResult);
}

#endif //_linterp_h


#include "testsuite.h"
#include <blitz/array.h>

using namespace blitz;

typedef TinyVector<int,3> tv3;

int main()
{
  tv3 A, B;
    A = 1, 2, 3;
    B = 0, 1, 2;

    BZTEST(A[0]==1);
    BZTEST(A[1]==2);
    BZTEST(A[2]==3);
    BZTEST(B[0]==0);
    BZTEST(B[1]==1);
    BZTEST(B[2]==2);

    A += B*B;
    BZTEST(all(A==tv3(1,3,7)));

    //cout << A << endl;

    A *= 10;
    BZTEST(all(A==tv3(10,30,70)));

    //cout << A << endl;

    tv3::iterator it=B.begin(), end=B.end();
    cout << "(" << *it;
    for (it++; it != end; ++it)
        cout << "," << *it;
    cout << ")" << endl;

// test tinyvector dot(), product(), and sum() functions

    int dotAB = dot(A,B);
    BZTEST(dotAB==170);
    int dotAB2 = dot(A,B+B);
    BZTEST(dotAB2==2*dotAB);
    int dotA2B = dot(A+A,B);
    BZTEST(dotA2B==2*dotAB);
    int dotA2B2 = dot(A+A,B+B);
    BZTEST(dotA2B2==4*dotAB);
    int prod1 = product(A);
    BZTEST(prod1==21000);
    int prod2 = product(A+B);
    BZTEST(prod2==22320);
    int sum1 = sum(A);
    BZTEST(sum1==110);
    int sum2 = sum(A-B);
    BZTEST(sum2==sum1-sum(B));

    // (cross product is tested in levicivita.cpp)

    // test funcs
    B=-1,-2,-3;
    A=abs(B);
    BZTEST(A[0]==1);
    BZTEST(A[1]==2);
    BZTEST(A[2]==3);

    TinyVector<double,3> C,D;
    D=1,4,9;
    C=sqrt(D);
    BZTEST(C[0]==1);
    BZTEST(C[1]==2);
    BZTEST(C[2]==3);

    // test expr constructor
    tv3 E(A+B*B);
    BZTEST(all(E==tv3(2,6,12)));

    return 0;
}


//  (C) Copyright Nick Thompson 2019.
//  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_STATISTICS_T_TEST_HPP
#define BOOST_MATH_STATISTICS_T_TEST_HPP

#include <cmath>
#include <iterator>
#include <utility>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>

namespace boost::math::statistics {

template<typename Real>
std::pair<Real, Real> one_sample_t_test(Real sample_mean, Real sample_variance, Real num_samples, Real assumed_mean) {
    using std::sqrt;
    typedef boost::math::policies::policy<
          boost::math::policies::promote_float<false>,
          boost::math::policies::promote_double<false> >
          no_promote_policy;

    Real test_statistic = (sample_mean - assumed_mean)/sqrt(sample_variance/num_samples);
    auto student = boost::math::students_t_distribution<Real, no_promote_policy>(num_samples - 1);
    Real pvalue;
    if (test_statistic > 0) {
        pvalue = 2*boost::math::cdf<Real>(student, -test_statistic);;
    }
    else {
        pvalue = 2*boost::math::cdf<Real>(student, test_statistic);
    }
    return std::make_pair(test_statistic, pvalue);
}

template<class ForwardIterator>
auto one_sample_t_test(ForwardIterator begin, ForwardIterator end, typename std::iterator_traits<ForwardIterator>::value_type assumed_mean) {
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    auto [mu, s_sq] = mean_and_sample_variance(begin, end);
    return one_sample_t_test(mu, s_sq, Real(std::distance(begin, end)), assumed_mean);
}

template<class Container>
auto one_sample_t_test(Container const & v, typename Container::value_type assumed_mean) {
    return one_sample_t_test(v.begin(), v.end(), assumed_mean);
}

}
#endif


#include "game/vsr/vsr.h"
#include <Eigen/Core>
#include <adept.h>
#include <hep/ga.hpp>
#include <iostream>

using vsr::cga::Vector;
using vsr::cga::Point;
using vsr::cga::Rotor;
using vsr::cga::Translator;

const double kPi = 3.141592653589793238462643383279;

template <typename T> using Matrix4 = Eigen::Matrix<T, 4, 4>;

template <typename T> inline static Matrix4<T> s() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(1), T(0),
      T(0), T(0), T(0), T(1);
  return m;
}

template <typename T> inline static Matrix4<T> e1() {
  Matrix4<T> m;
  m << T(0), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0),
      T(1), T(0), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e2() {
  Matrix4<T> m;
  m << T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(1), T(0), T(0), T(0),
      T(0), T(-1), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e3() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(0),
      T(0), T(0), T(0), T(-1);
  return m;
}

template <typename T> inline static Matrix4<T> e23() {
  return e2<T>() * e3<T>();
}

template <typename T> inline static Matrix4<T> e31() {
  return e3<T>() * e1<T>();
}

template <typename T> inline static Matrix4<T> e12() {
  return e1<T>() * e2<T>();
}

template <typename T> inline static Matrix4<T> e123() {
  return e1<T>() * e2<T>() * e3<T>();
}

template <typename T> void Diff4(const T th, const T *a, T *b) {
  Matrix4<T> rotor =
      cos(T(0.5) * th) * s<T>() - sin(T(0.5) * th) * e1<T>() * e2<T>();
  Matrix4<T> rotor_inv =
      cos(T(0.5) * th) * s<T>() + sin(T(0.5) * th) * e1<T>() * e2<T>();
  Matrix4<T> vec_a = a[0] * e1<T>() + a[1] * e2<T>() + a[2] * e3<T>();
  Matrix4<T> vec_b = rotor * vec_a * rotor_inv;
  b[0] = vec_b(0, 3);
  b[1] = vec_b(2, 0);
  b[2] = vec_b(0, 0);
}

template <typename T> void Diff5(const T th, const T *a, T *b) {
  using Algebra = hep::algebra<T, 3, 0>;
  using Rotor = hep::multi_vector<Algebra, hep::list<0, 3, 5, 6>>;
  using Vector = hep::multi_vector<Algebra, hep::list<1, 2, 4>>;
  Rotor rotor{cos(T(0.5) * th), -sin(T(0.5) * th), T(0.0), T(0.0)};
  Vector pnt_a{a[0], a[1], a[2]};
  Vector pnt_b = hep::grade<1>(rotor * pnt_a * ~rotor);
  for (int i = 0; i < 3; ++i)
    b[i] = pnt_b[i];
}

template <typename T> void Diff(const T th, const T *a, T *b) {
  Rotor<T> rotor{cos(T(0.5) * th), -sin(T(0.5) * th), T(0.0), T(0.0)};
  Vector<T> vec_a{a[0], a[1], a[2]};
  Vector<T> vec_b = vec_a.spin(rotor);
  for (int i = 0; i < 3; ++i)
    b[i] = vec_b[i];
}

template <typename T> void Diff2(const T th, const T *a, T *b) {
  Rotor<T> rotor{cos(T(0.5) * th), -sin(T(0.5) * th), T(0.0), T(0.0)};
  Point<T> pnt_a = Vector<T>{a[0], a[1], a[2]}.null();
  Point<T> pnt_b = pnt_a.spin(rotor);
  for (int i = 0; i < 5; ++i)
    b[i] = pnt_b[i];
}

template <typename T> void Diff3(const T t, const T *a, T *b) {
  Translator<T> trs{T(1.0), -T(0.5) * t, T(0.0), T(0.0)};
  Point<T> pnt_a = Vector<T>{a[0], a[1], a[2]}.null();
  Point<T> pnt_b = pnt_a.spin(trs);
  for (int i = 0; i < 5; ++i)
    b[i] = pnt_b[i];
}

int main() {
  adept::Stack stack;
  const double theta = kPi / 3;

  double jac[5];
  adept::adouble at = 3.0;
  adept::adouble atheta = theta;
  adept::adouble a[3] = {1.0, 0.0, 0.0}; // e1
  stack.new_recording();
  adept::adouble b[5] = {0.0, 0.0, 0.0, 0.0, 0.0};

  // Diff(atheta, a, b);
  Diff4(atheta, a, b);
  // Diff5(atheta, a, b);
  // Diff2(at, a, b);
  // Diff3(at, a, b);

  // stack.independent(&at, 1);
  stack.independent(&atheta, 1);
  stack.dependent(b, 3);
  stack.jacobian_reverse(jac);
  // stack.jacobian_forward(jac);

  for (int i = 0; i < 3; ++i)
    std::cout << b[i] << " " << std::endl;

  for (int i = 0; i < 3; ++i)
    std::cout << jac[i] << " " << std::endl;

  stack.print_status();
  // stack.print_statements();
  // stack.print_gradients();

  return 0;
}


#include <Eigen/Core>
#include <iostream>
#include <mathtoolbox/rbf-interpolation.hpp>
#include <random>
#include <timer.hpp>
#include <vector>

using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;

namespace
{
    std::random_device                     seed;
    std::default_random_engine             engine(seed());
    std::uniform_real_distribution<double> uniform_dist(-1.0, 1.0);

    double CalcFunction(const Vector2d& x) { return std::sin(10.0 * x(0)) + std::sin(10.0 * x(1)); }

    std::shared_ptr<timer::Timer> timer_object;
} // namespace

void PerformTest()
{
    constexpr int    number_of_samples      = 500;
    constexpr int    number_of_test_samples = 100;
    constexpr double noise_intensity        = 0.1;

    // Generate scattered data (in this case, 500 data points in a 2-dimensional space)
    Eigen::MatrixXd X = Eigen::MatrixXd::Random(2, number_of_samples);
    Eigen::VectorXd y(number_of_samples);
    for (int i = 0; i < number_of_samples; ++i)
    {
        y(i) = CalcFunction(X.col(i)) + noise_intensity * uniform_dist(engine);
    }

    // Define interpolation settings
    const auto     kernel             = mathtoolbox::ThinPlateSplineRbfKernel();
    constexpr bool use_regularization = true;

    // Instantiate an interpolator
    mathtoolbox::RbfInterpolator rbf_interpolator(kernel);

    // Set data
    rbf_interpolator.SetData(X, y);

    // Calculate internal weights with or without regularization
    timer_object = std::make_shared<timer::Timer>("CalcWeights()");
    rbf_interpolator.CalcWeights(use_regularization);
    timer_object = nullptr;

    // Calculate interpolated values on randomly sampled points
    Eigen::MatrixXd X_test = Eigen::MatrixXd::Random(2, number_of_test_samples);
    Eigen::VectorXd y_test(number_of_test_samples);
    timer_object = std::make_shared<timer::Timer>("CalcValue() * " + std::to_string(number_of_test_samples));
    for (int i = 0; i < number_of_test_samples; ++i)
    {
        y_test(i) = rbf_interpolator.CalcValue(X_test.col(i));
    }
    timer_object = nullptr;

    // Display the results in the CSV format
    std::cout << "x(0),x(1),y" << std::endl;
    for (int i = 0; i < number_of_test_samples; ++i)
    {
        std::cout << X_test(0, i) << "," << X_test(1, i) << "," << y_test(i) << std::endl;
    }
}

int main()
{
    PerformTest();

    return 0;
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/significants.hpp>
#include <boost/simd/function/simd/enumerate.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;
namespace bd =  boost::dispatch;
template < int N >
struct signif
{
  template<class T> T operator()(const T & a) const
  {
    using i_t = bd::as_integer_t<T>;
    return bs::significants(a, bs::enumerate<i_t>(0, N));
  }
};

DEFINE_SIMD_BENCH(simd_significantsp, signif< 1>());
DEFINE_SIMD_BENCH(simd_significantsn, signif<-1>());

DEFINE_BENCH_MAIN() {
  nsb::for_each<simd_significantsn, NS_BENCH_IEEE_TYPES>(-10, 10);
  nsb::for_each<simd_significantsp, NS_BENCH_IEEE_TYPES>(-10, 10);
}


#include "sphere.h"

#include <boost\math\constants\constants.hpp>

void make_sphere(std::vector<glm::vec3> &vertices, std::vector<glm::vec3> &normals, std::vector<glm::vec2> &stCoordinates, std::vector<GLushort> &elements, int rings, int sectors)
{
	int radius = 1;

	float const R = 1. / (float) (rings - 1);
	float const S = 1. / (float) (sectors - 1);
	int r, s;

	vertices.resize(rings * sectors);
	normals.resize(rings * sectors);
	stCoordinates.resize(rings * sectors);
	std::vector<glm::vec3>::iterator v = vertices.begin();
	std::vector<glm::vec3>::iterator n = normals.begin();
	std::vector<glm::vec2>::iterator t = stCoordinates.begin();
	for (r = 0; r < rings; r++) for (s = 0; s < sectors; s++) {
		float const y = sin(-boost::math::float_constants::half_pi + boost::math::float_constants::pi * r * R);
		float const x = cos(2 * boost::math::float_constants::pi * s * S) * sin(boost::math::float_constants::pi * r * R);
		float const z = sin(2 * boost::math::float_constants::pi * s * S) * sin(boost::math::float_constants::pi * r * R);

		*t++ = glm::vec2(s*S, r*R);

		*v++ = glm::vec3(x * radius, y * radius, z * radius);

		*n++ = glm::vec3(x, y, z);
	}

	elements.resize(rings * sectors * 6);
	std::vector<GLushort>::iterator i = elements.begin();
	for (r = 0; r < rings - 1; r++) for (s = 0; s < sectors - 1; s++) {
		*i++ = r * sectors + s; //0
		*i++ = (r + 1) * sectors + (s + 1); //2
		*i++ = r * sectors + (s + 1); //1
		*i++ = r * sectors + s; //0
		*i++ = (r + 1) * sectors + s; //3
		*i++ = (r + 1) * sectors + (s + 1); //2
	}
}


/*! \file 2d_limit.cpp
  \brief Simple 2D plot show 1/x function values at limit.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2009, 2021
// Distributed under the Boost Software License, Version 1.0.
// For more information, see http://www.boost.org

#include <boost/svg_plot/svg_2d_plot.hpp>
// boost::svg::svg_2d_plot
#include <limits>
// infinity
#include <map>
//using std::map;

double f(double x)
{ // Function to plot.
  return 1. / x;
}

int main()
{
  using namespace boost::svg; // For SVG named colors.
  std::map<double, double> data1;

  const double interval = 0.5;
  for(double i = -10; i <= 10.; i += interval)
  {
    data1[i] = f(i);
  }

  svg_2d_plot my_plot;

  // Image size & ranges settings.
  my_plot.size(500, 350) // SVG image in pixels.
         .x_range(-10.5, 10.5) // Offset by 0.5 so that +10 and -10 markers are visible.
         .y_range(-1.1, 1.1); // Offset by 1 so that +10 and -10 markers are visible.

  // Text settings.
  my_plot.title("Plot of 1 / x")
    .x_label("X Axis Units")
    .y_label("F(x)")
    .y_major_labels_side(-1) // Left.
    .plot_window_on(true);

  // X-axis settings.
  my_plot.x_major_interval(2)
    .x_major_tick_length(14)
    .x_major_tick_width(1)
    .x_minor_tick_length(7)
    .x_minor_tick_width(1)
    .x_num_minor_ticks(1)
    .x_major_labels_side(1) // Top of X-axis line (but zero collides with vertical x == 0 line).

  // Y-axis settings.
         .y_major_interval(1)
         .y_num_minor_ticks(4);

  // Legend-box settings.
  my_plot.legend_title_font_size(15);

  // Limit value at x = 0 when 1/x == +infinity shown by a pointing-down cone a the top of the plot.
  //my_plot.minus_inf_limit_color(red).plus_inf_limit_color(green);
  // TODO - the 'at-limit' infinity point cone pointing-down shows as default color pink, not the changed color(s).

  my_plot.plot(data1, "1 / x").shape(square).size (5).line_on(false);

  my_plot.write("./2d_limit.svg");

  // Sets and gets colors correctly, but not used? A fix would be good, but does display.
  // std::cout << "" << my_plot.plus_inf_limit_color() << std::endl; // RGB(0,128,0)
   //std::cout << "" << my_plot.minus_inf_limit_color() << std::endl; // RGB(255,0,0)

  return 0;
} // int main()


//
//  Polygone.cpp
//
//
//  Created by Victoire Chapelle on 14/01/2016.
//
//

#include "Polygon.h"

#include <math.h>
#include <string>

#include <boost/serialization/vector.hpp>

#include "Utils.h"
#include "optional.h"
#include "Segment.h"

namespace TP4
{
	std::experimental::optional<Polygon> make_polygon(const std::vector<Point> vertices)
	{
		if (vertices.size() > 2)
		{
			Point p1, p2;

			p1.first = vertices[0].first - vertices[vertices.size() - 1].first;
			p1.second = vertices[0].second - vertices[vertices.size() - 1].second;
			p2.first = vertices[1].first - vertices[0].first;
			p2.second = vertices[1].second - vertices[0].second;

			double det_value;
			double current_det_value;

			det_value = p1.first * p2.second - p1.second * p2.first;

			for (size_t i = 0; i < vertices.size(); ++i)
			{
				p1.first = vertices[mod(i + 1, vertices.size())].first - vertices[i].first;
				p1.second = vertices[mod(i + 1, vertices.size())].second - vertices[i].second;
				p2.first = vertices[mod(i + 2, vertices.size())].first - vertices[mod(i + 1, vertices.size())].first;
				p2.second = vertices[mod(i + 2, vertices.size())].second - vertices[mod(i + 1, vertices.size())].second;

				current_det_value = p1.first * p2.second - p1.second * p2.first;

				if (det_value*current_det_value < 0 - 0.00001)
					return std::experimental::nullopt;
			}

			return std::experimental::optional<Polygon>(Polygon(std::move(vertices)));
		}

		return std::experimental::nullopt;
	}

	Polygon Move(const Polygon& shape, coord_t dx, coord_t dy)
	{
		std::vector<Point> new_vertices;
		new_vertices.reserve(new_vertices.size());

		for (const Point& p : shape.vertices)
			new_vertices.emplace_back(p.first + dx, p.second + dy);

		return Polygon(std::move(new_vertices));
	}

	inline double squared_distance(Point a, Point b)
	{
		return (a.first - b.first) * (a.first - b.first)
			+ (a.second - b.second) * (a.second - b.second);
	}

	bool Is_contained(const Polygon& shape, Point point)
	{
		double sum = 0.0;

		// Calcul de la somme des cosinus que font les angles entre les sommets consécutifs et le point
		for (size_t i = 0; i < shape.vertices.size(); ++i)
		{
			auto next_point = shape.vertices[mod(i + 1, shape.vertices.size())];

			double a2 = squared_distance(shape.vertices[i], point);
			double c2 = squared_distance(shape.vertices[i], next_point);
			double b2 = squared_distance(next_point, point);

			if (abs(a2) < 0.001 || abs(b2) < 0.001)
				return true; // Le point est confondu avec un sommet

			auto seg = make_segment(shape.vertices[i], next_point);
			if(seg)
				if (Is_contained(*seg, point))
					return true;

			// Formule d'al-kashi : cos(angle) = (a² + b² - c²) / (2*a*b)
			double cos = (a2 + b2 - c2) / (2.0*sqrt(a2)*sqrt(b2));
		//	double cos = c2 / (length_1 + length_3 - 2.0*sqrt(length_3)*sqrt(length_1));

			sum += acos(cos);
		}

		// Si la somme est 1 le point est à l'intérieur (somme des angles = 360°)
		// TODO: faire mieux?
		if (abs(mod(sum, 2.0*3.14159) < 0.001))
			return true;
		return false;
	}

	Polygon::Polygon(const std::vector<Point>&& vertices)
		: vertices(std::move(vertices))
	{ }

	std::ostream& operator<<(std::ostream& flux, const Polygon& polygone)
	{
		flux << "{ ";
		for (size_t i = 0; i < polygone.vertices.size(); ++i)
			flux << "(" << polygone.vertices[i].first << ", " << polygone.vertices[i].second << (i == polygone.vertices.size() - 1 ? ") " : "); ");
		flux << "}";

		return flux;
	}
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/containers/mesh/polymesh/polymesh.h>
#include <OpenTissue/core/containers/mesh/polymesh/util/polymesh_is_point_inside.h>
#include <OpenTissue/core/containers/mesh/common/util/mesh_make_box.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

BOOST_AUTO_TEST_SUITE(opentissue_polymesh_is_point_inside);

BOOST_AUTO_TEST_CASE(box_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::matrix3x3_type                       matrix3x3_type;
  typedef math_types::real_type                            real_type;

  OpenTissue::polymesh::PolyMesh<math_types> mesh;

  OpenTissue::mesh::make_box(1.0,1.0,1.0,mesh);

  //std::vector<vector3_type> profile;
  //profile.push_back(vector3_type(0.0,0.0,0.0));
  //profile.push_back(vector3_type(5.0,0.0,5.0));
  //profile.push_back(vector3_type(5.0,0.0,10.0));
  //profile.push_back(vector3_type(0.0,0.0,15.0));
  //OpenTissue::mesh::profile_sweep(profile.begin(),profile.end(),2*OpenTissue::math::detail::pi<real_type>(),32,m_mesh);

  bool inside = false;
  vector3_type p;
  real_type val = 0.0;

  p = vector3_type(0.0,0.0,0.0);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );

  val = 0.45;
  p = vector3_type(val,val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(val,val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(val,-val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(val,-val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(-val,val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(-val,val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(-val,-val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );
  p = vector3_type(-val,-val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( inside );

  val = 0.8;
  p = vector3_type(val,val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(val,val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(val,-val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(val,-val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(-val,val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(-val,val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(-val,-val,val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );
  p = vector3_type(-val,-val,-val);
  inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  BOOST_CHECK( !inside );

}

BOOST_AUTO_TEST_CASE(advanced_test)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::matrix3x3_type                       matrix3x3_type;
  typedef math_types::real_type                            real_type;

  OpenTissue::polymesh::PolyMesh<math_types> mesh;

  //std::vector<vector3_type> profile;
  //profile.push_back(vector3_type(0.0,0.0,0.0));
  //profile.push_back(vector3_type(5.0,0.0,5.0));
  //profile.push_back(vector3_type(5.0,0.0,10.0));
  //profile.push_back(vector3_type(0.0,0.0,15.0));
  //OpenTissue::mesh::profile_sweep(profile.begin(),profile.end(),2*OpenTissue::math::detail::pi<real_type>(),32,m_mesh);

  //bool inside = false;
  vector3_type p;

  //p = vector3_type(0.0,0.0,0.0);
  //inside = OpenTissue::polymesh::is_point_inside( mesh, p );
  //BOOST_CHECK( inside );
}

BOOST_AUTO_TEST_SUITE_END();


/* test_extreme_value.cpp
 *
 * Copyright Steven Watanabe 2010
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: test_extreme_value.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/extreme_value_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/extreme_value.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::extreme_value_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME extreme_value
#define BOOST_MATH_DISTRIBUTION boost::math::extreme_value
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME a
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME b
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


#include "ols.h"

#include <carma>
#include <armadillo>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/pytypes.h>

py::tuple ols(arma::colvec& y, arma::mat& X) {
    int n = X.n_rows, k = X.n_cols;

    arma::colvec coeffs = arma::solve(X, y);
    arma::colvec resid = y - X * coeffs;

    double sig2 = arma::as_scalar(arma::trans(resid) * resid / (n-k));
    arma::colvec std_errs = arma::sqrt(sig2 * arma::diagvec( arma::inv(arma::trans(X)*X)) );

    return py::make_tuple(
        carma::col_to_arr(coeffs),
        carma::col_to_arr(std_errs)
    );
}

void bind_ols(py::module &m) {
    m.def(
        "ols",
        &ols,
        R"pbdoc(
            Example function performing OLS.

            Parameters
            ----------
            arr : np.array
                input array

            Returns
            -------
            coeffs: np.ndarray
                coefficients
            std_err : np.ndarray
                standard error on the coefficients
        )pbdoc",
        py::arg("y"),
        py::arg("x")
    );
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_BUNDLE_COST_HPP__
#define __OPTIMISATION2_BUNDLE_COST_HPP__

#include <cmath>
#include "../function/function.hpp"
#include <libv/lma/ttt/traits/wrap.hpp>
#include <boost/mpl/for_each.hpp>
#include "../ba/nan_error.hpp"
#include "../ba/mat.hpp"
#include "../omp/omp.hpp"

namespace lma
{
  template<class Obs, class Bundle> double cost(const Bundle& bundle)
  {
    const auto nb_obs = bundle.template at_obs<Obs>().size();
    if (nb_obs==0) return 0;

//     std::cout << " cost without save " << std::endl;

    double total = 0;

//     #pragma omp parallel for reduction(+:total) if(use_omp())
    for(auto iobs = bundle.template at_obs<Obs>().first() ; iobs < nb_obs ; ++iobs)
    {
      //total += (make_function(bundle.obs(iobs))(bundle.map(iobs))).squaredNorm() / 2;
      auto pair_residu = make_function(bundle.obs(iobs))(bundle.map(iobs));
      if (pair_residu.second)
        total += ( pair_residu ).first.squaredNorm();
    }

    if (std::isnan(total))
      throw NAN_ERROR(" NAN : cost_and_save");
    return total/2.0;
  }

  template<class Bundle> struct Coster
  {
    const Bundle& bundle;
    double sum;
    Coster(const Bundle& bundle_):bundle(bundle_),sum(0){}

    template<class T> void operator()(ttt::wrap<T>)
    {
      sum += cost<T,Bundle>(bundle);
    }
  };

  // total square error
  template<class Bundle> double cost(const Bundle& bundle)
  {
    Coster<Bundle> coster(bundle);
    mpl::for_each<typename Bundle::ListeToObs, ttt::wrap<mpl::placeholders::_1>>(boost::ref(coster));
    return coster.sum;
  }

  template<class ListeObs, class Bundle> double costs(const Bundle& bundle)
  {
    Coster<Bundle> coster(bundle);
    mpl::for_each<ListeObs, ttt::wrap<mpl::placeholders::_1>>(boost::ref(coster));
    return coster.sum;
  }

  // root mean square
  template<class Bundle> double rms(const Bundle& bundle)
  {
    if (bundle.nb_obs()==0) return 0;
    return  sqrt( cost(bundle) / (double)(bundle.nb_obs()));
  }

  // mean square error
  template<class Bundle> double mse(const Bundle& bundle)
  {
    if (bundle.nb_obs()==0) return 0;
    return   cost(bundle) * 2.0 / (double)(bundle.nb_obs());
  }

}//eon

#endif


#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>

#include "common.h"
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>

#include <Eigen/Dense>

using namespace Sophus;
using namespace Eigen;
using namespace std;

struct Camera
{
    Camera() {}

    Camera(double* data)
    {
        R = Sophus::SO3d::exp(Eigen::Vector3d(data[0], data[1], data[2]));
        t = Eigen::Vector3d(data[3], data[4], data[5]);
        f = data[6];
        k1 = data[7];
        k2 = data[8];
    }

    void set_to(double* data) const
    {
        Eigen::Vector3d r = R.log();
        for (int i = 0; i < 3; ++i)
        {
            data[i] = r[i];
            data[i+3] = t[i];
        }
        data[6] = f;
        data[7] = k1;
        data[8] = k2;
    }

    Sophus::SO3d R;
    Eigen::Vector3d t = Eigen::Vector3d::Zero();
    double f = 0.0, k1 = 0.0, k2 = 0.0;
};

class VertexCamera: public g2o::BaseVertex<9, Camera>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Camera();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate.R = Sophus::SO3d::exp(Eigen::Vector3d(update[0], update[1], update[2])) * _estimate.R;
        _estimate.t += Eigen::Map<const Eigen::Vector3d>(update+3);
        _estimate.f += update[6];
        _estimate.k1 += update[7];
        _estimate.k2 += update[8];
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class VertexLandmark: public g2o::BaseVertex<3, Eigen::Vector3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Vector3d::Zero();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Map<const Eigen::Vector3d>(update);
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class EdgeReprojection: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexCamera, VertexLandmark>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeReprojection() { }

    virtual void computeError() override {
        const VertexCamera* v_cam = static_cast<VertexCamera*>(_vertices[0]);
        const VertexLandmark* v_point = static_cast<VertexLandmark*>(_vertices[1]);
        auto cam = v_cam->estimate();
        Eigen::Vector3d X = v_point->estimate();
        Eigen::Vector3d X_cam = (cam.R * X) + cam.t;
        X_cam /= -X_cam.z(); // minus because of dataset
        auto p2 = X_cam.x() * X_cam.x() + X_cam.y() * X_cam.y();
        auto r = 1.0 + p2 * (cam.k1 + (p2 * cam.k2));

        Eigen::Vector2d uv = X_cam.head<2>() * r * cam.f;
        _error = _measurement - uv;
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}

};

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

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem dataset(argv[1]);
    dataset.Normalize();
    dataset.Perturb(0.1, 0.5, 0.5);
    dataset.WriteToPLYFile("initial_pc.ply");

    std::cout << "\n";
    std::cout << "nb cameras: " << dataset.num_cameras() << std::endl;
    std::cout << "nb landmarks: " << dataset.num_points() << std::endl;
    std::cout << "nb observations: " << dataset.num_observations() << std::endl;
    std::cout << "nb parameters: " << dataset.num_parameters() << std::endl;
    std::cout << "check: " << dataset.num_cameras() * 9 + dataset.num_points()*3 << std::endl;

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;

    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
    );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto* cameras = dataset.mutable_cameras();
    std::vector<VertexCamera*> camera_vertices;
    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        auto *c = new VertexCamera();
        c->setId(i);
        c->setEstimate(Camera(cameras + (i*dataset.camera_block_size())));
        optimizer.addVertex(c);
        camera_vertices.push_back(c);
    }

    auto* landmarks = dataset.mutable_points();
    std::vector<VertexLandmark*> landmark_vertices;
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        auto* l = new VertexLandmark();
        l->setId(dataset.num_cameras() + i);
        l->setEstimate(Eigen::Map<Eigen::Vector3d>(landmarks + i*dataset.point_block_size()));
        l->setMarginalized(true);
        optimizer.addVertex(l);
        landmark_vertices.push_back(l);
    }

    auto* observations = dataset.observations();
    auto* cam_indices = dataset.camera_index();
    auto* landmark_indices = dataset.point_index();
    for (int i = 0; i < dataset.num_observations(); ++i)
    {
        auto* e = new EdgeReprojection();
        e->setVertex(0, camera_vertices[cam_indices[i]]);
        e->setVertex(1, landmark_vertices[landmark_indices[i]]);
        e->setMeasurement(Eigen::Map<const Eigen::Vector2d>(observations + i*2));
        e->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(e);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        camera_vertices[i]->estimate().set_to(cameras + (i * dataset.camera_block_size()));
    }
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        Eigen::Vector3d X = landmark_vertices[i]->estimate();
        landmarks[i*3] = X.x();
        landmarks[i*3+1] = X.y();
        landmarks[i*3+2] = X.z();
    }

    dataset.WriteToPLYFile("after_ba_g2o.ply");

    return 0;
}


// Copyright Jim Bosch 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/python/numpy.hpp>

#include <cmath>
#include <memory>

#ifndef M_PI
#include <boost/math/constants/constants.hpp>
const double M_PI = boost::math::constants::pi<double>();
#endif

namespace bp = boost::python;
namespace bn = boost::python::numpy;

/**
 *  A 2x2 matrix class, purely for demonstration purposes.
 *
 *  Instead of wrapping this class with Boost.Python, we'll convert it to/from numpy.ndarray.
 */
class matrix2 {
public:

    double & operator()(int i, int j) {
        return _data[i*2 + j];
    }

    double const & operator()(int i, int j) const {
        return _data[i*2 + j];
    }

    double const * data() const { return _data; }

private:
    double _data[4];
};

/**
 *  A 2-element vector class, purely for demonstration purposes.
 *
 *  Instead of wrapping this class with Boost.Python, we'll convert it to/from numpy.ndarray.
 */
class vector2 {
public:

    double & operator[](int i) {
        return _data[i];
    }

    double const & operator[](int i) const {
        return _data[i];
    }

    double const * data() const { return _data; }

    vector2 operator+(vector2 const & other) const {
        vector2 r;
        r[0] = _data[0] + other[0];
        r[1] = _data[1] + other[1];
        return  r;
    }

    vector2 operator-(vector2 const & other) const {
        vector2 r;
        r[0] = _data[0] - other[0];
        r[1] = _data[1] - other[1];
        return  r;
    }

private:
    double _data[2];
};

/**
 *  Matrix-vector multiplication.
 */
vector2 operator*(matrix2 const & m, vector2 const & v) {
    vector2 r;
    r[0] = m(0, 0) * v[0] + m(0, 1) * v[1];
    r[1] = m(1, 0) * v[0] + m(1, 1) * v[1];
    return r;
}

/**
 *  Vector inner product.
 */
double dot(vector2 const & v1, vector2 const & v2) {
    return v1[0] * v2[0] + v1[1] * v2[1];
}

/**
 *  This class represents a simple 2-d Gaussian (Normal) distribution, defined by a
 *  mean vector 'mu' and a covariance matrix 'sigma'.
 */
class bivariate_gaussian {
public:

    vector2 const & get_mu() const { return _mu; }

    matrix2 const & get_sigma() const { return _sigma; }

    /**
     *  Evaluate the density of the distribution at a point defined by a two-element vector.
     */
    double operator()(vector2 const & p) const {
        vector2 u = _cholesky * (p - _mu);
        return 0.5 * _cholesky(0, 0) * _cholesky(1, 1) * std::exp(-0.5 * dot(u, u)) / M_PI;
    }

    /**
     *  Evaluate the density of the distribution at an (x, y) point.
     */
    double operator()(double x, double y) const {
        vector2 p;
        p[0] = x;
        p[1] = y;
        return operator()(p);
    }

    /**
     *  Construct from a mean vector and covariance matrix.
     */
    bivariate_gaussian(vector2 const & mu, matrix2 const & sigma)
        : _mu(mu), _sigma(sigma), _cholesky(compute_inverse_cholesky(sigma))
    {}

private:

    /**
     *  This evaluates the inverse of the Cholesky factorization of a 2x2 matrix;
     *  it's just a shortcut in evaluating the density.
     */
    static matrix2 compute_inverse_cholesky(matrix2 const & m) {
        matrix2 l;
        // First do cholesky factorization: l l^t = m
        l(0, 0) = std::sqrt(m(0, 0));
        l(0, 1) = m(0, 1) / l(0, 0);
        l(1, 1) = std::sqrt(m(1, 1) - l(0,1) * l(0,1));
        // Now do forward-substitution (in-place) to invert:
        l(0, 0) = 1.0 / l(0, 0);
        l(1, 0) = l(0, 1) = -l(0, 1) / l(1, 1);
        l(1, 1) = 1.0 / l(1, 1);
        return l;
    }

    vector2 _mu;
    matrix2 _sigma;
    matrix2 _cholesky;

};

/*
 *  We have a two options for wrapping get_mu and get_sigma into NumPy-returning Python methods:
 *   - we could deep-copy the data, making totally new NumPy arrays;
 *   - we could make NumPy arrays that point into the existing memory.
 *  The latter is often preferable, especially if the arrays are large, but it's dangerous unless
 *  the reference counting is correct: the returned NumPy array needs to hold a reference that
 *  keeps the memory it points to from being deallocated as long as it is alive.  This is what the
 *  "owner" argument to from_data does - the NumPy array holds a reference to the owner, keeping it
 *  from being destroyed.
 *
 *  Note that this mechanism isn't completely safe for data members that can have their internal
 *  storage reallocated.  A std::vector, for instance, can be invalidated when it is resized,
 *  so holding a Python reference to a C++ class that holds a std::vector may not be a guarantee
 *  that the memory in the std::vector will remain valid.
 */

/**
 *  These two functions are custom wrappers for get_mu and get_sigma, providing the shallow-copy
 *  conversion with reference counting described above.
 *
 *  It's also worth noting that these return NumPy arrays that cannot be modified in Python;
 *  the const overloads of vector::data() and matrix::data() return const references,
 *  and passing a const pointer to from_data causes NumPy's 'writeable' flag to be set to false.
 */
static bn::ndarray py_get_mu(bp::object const & self) {
    vector2 const & mu = bp::extract<bivariate_gaussian const &>(self)().get_mu();
    return bn::from_data(
        mu.data(),
        bn::dtype::get_builtin<double>(),
        bp::make_tuple(2),
        bp::make_tuple(sizeof(double)),
        self
    );
}
static bn::ndarray py_get_sigma(bp::object const & self) {
    matrix2 const & sigma = bp::extract<bivariate_gaussian const &>(self)().get_sigma();
    return bn::from_data(
        sigma.data(),
        bn::dtype::get_builtin<double>(),
        bp::make_tuple(2, 2),
        bp::make_tuple(2 * sizeof(double), sizeof(double)),
        self
    );
}

/**
 *  To allow the constructor to work, we need to define some from-Python converters from NumPy arrays
 *  to the matrix/vector types.  The rvalue-from-python functionality is not well-documented in Boost.Python
 *  itself; you can learn more from boost/python/converter/rvalue_from_python_data.hpp.
 */

/**
 *  We start with two functions that just copy a NumPy array into matrix/vector objects.  These will be used
 *  in the templated converted below.  The first just uses the operator[] overloads provided by
 *  bp::object.
 */
static void copy_ndarray_to_mv2(bn::ndarray const & array, vector2 & vec) {
    vec[0] = bp::extract<double>(array[0]);
    vec[1] = bp::extract<double>(array[1]);
}

/**
 *  Here, we'll take the alternate approach of using the strides to access the array's memory directly.
 *  This can be much faster for large arrays.
 */
static void copy_ndarray_to_mv2(bn::ndarray const & array, matrix2 & mat) {
    // Unfortunately, get_strides() can't be inlined, so it's best to call it once up-front.
    Py_intptr_t const * strides = array.get_strides();
    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j < 2; ++j) {
            mat(i, j) = *reinterpret_cast<double const *>(array.get_data() + i * strides[0] + j * strides[1]);
        }
    }
}

/**
 *  Here's the actual converter.  Because we've separated the differences into the above functions,
 *  we can write a single template class that works for both matrix2 and vector2.
 */
template <typename T, int N>
struct mv2_from_python {

    /**
     *  Register the converter.
     */
    mv2_from_python() {
        bp::converter::registry::push_back(
            &convertible,
            &construct,
            bp::type_id< T >()
        );
    }

    /**
     *  Test to see if we can convert this to the desired type; if not return zero.
     *  If we can convert, returned pointer can be used by construct().
     */
    static void * convertible(PyObject * p) {
        try {
            bp::object obj(bp::handle<>(bp::borrowed(p)));
            std::auto_ptr<bn::ndarray> array(
                new bn::ndarray(
                    bn::from_object(obj, bn::dtype::get_builtin<double>(), N, N, bn::ndarray::V_CONTIGUOUS)
                )
            );
            if (array->shape(0) != 2) return 0;
            if (N == 2 && array->shape(1) != 2) return 0;
            return array.release();
        } catch (bp::error_already_set & err) {
            bp::handle_exception();
            return 0;
        }
    }

    /**
     *  Finish the conversion by initializing the C++ object into memory prepared by Boost.Python.
     */
    static void construct(PyObject * obj, bp::converter::rvalue_from_python_stage1_data * data) {
        // Extract the array we passed out of the convertible() member function.
        std::auto_ptr<bn::ndarray> array(reinterpret_cast<bn::ndarray*>(data->convertible));
        // Find the memory block Boost.Python has prepared for the result.
        typedef bp::converter::rvalue_from_python_storage<T> storage_t;
        storage_t * storage = reinterpret_cast<storage_t*>(data);
        // Use placement new to initialize the result.
        T * m_or_v = new (storage->storage.bytes) T();
        // Fill the result with the values from the NumPy array.
        copy_ndarray_to_mv2(*array, *m_or_v);
        // Finish up.
        data->convertible = storage->storage.bytes;
    }

};

BOOST_PYTHON_MODULE(gaussian) {
    bn::initialize();

    // Register the from-python converters
    mv2_from_python< vector2, 1 >();
    mv2_from_python< matrix2, 2 >();

    typedef double (bivariate_gaussian::*call_vector)(vector2 const &) const;

    bp::class_<bivariate_gaussian>("bivariate_gaussian", bp::init<bivariate_gaussian const &>())

        // Declare the constructor (wouldn't work without the from-python converters).
        .def(bp::init< vector2 const &, matrix2 const & >())

        // Use our custom reference-counting getters
        .add_property("mu", &py_get_mu)
        .add_property("sigma", &py_get_sigma)

        // First overload accepts a two-element array argument
        .def("__call__", (call_vector)&bivariate_gaussian::operator())

        // This overload works like a binary NumPy universal function: you can pass
        // in scalars or arrays, and the C++ function will automatically be called
        // on each element of an array argument.
        .def("__call__", bn::binary_ufunc<bivariate_gaussian,double,double,double>::make())
        ;
}