#include <math.h>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "geometry.hpp"

namespace cuauv {
namespace fishbowl {

const double singularity_cutoff = M_PI/2 * 0.985;

// Sphere-sphere Sweep implementation from:
// http://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?page=2

bool quadratic(double a, double b, double c, double& r1, double& r2)
{
    double q = b * b - 4 * a * c;
    if (q < 0) {
        return false;
    } else {
        double sq = sqrt(q);
        double d = 1 / (2 * a);
        r1 = (-b + sq) * d;
        r2 = (-b - sq) * d;
        return true;
    }
}

bool sphere_sphere_sweep(double ar, const Eigen::Vector3d& ax0, const Eigen::Vector3d& ax1,
                         double br, const Eigen::Vector3d& bx0, const Eigen::Vector3d& bx1,
                         double& t0, double& t1)
{
    Eigen::Vector3d av = ax1 - ax0;
    Eigen::Vector3d bv = bx1 - bx0;
    Eigen::Vector3d ab = bx0 - ax0;
    Eigen::Vector3d abv = bv - av;
    double abr = ar + br;
    double a = abv.dot(abv);
    double b = 2 * abv.dot(ab);
    double c = ab.dot(ab) - abr * abr;

    if (ab.dot(ab) <= abr * abr) {
        t0 = 0;
        t1 = 0;
        return true;
    }

    if (quadratic(a, b, c, t0, t1)) {
        if (t0 > t1) {
            double x = t0;
            t0 = t1;
            t1 = x;
        }
        // t0 <= t1
        return (0 <= t0 && t0 <= 1) || (t0 <= 0 && t1 >= 0);
    }

    return false;
}

double line_distance(const Eigen::Vector3d& x0, const Eigen::Vector3d& x1, const Eigen::Vector3d& x)
{
    const Eigen::Vector3d p = x1 - x0;
    const Eigen::Vector3d y = x - x0;
    const Eigen::Vector3d o = (y.dot(p) / p.dot(p)) * p;
    const Eigen::Vector3d r = y - o;

    return r.dot(r);
}

void swing_twist(const Eigen::Quaterniond& q, const Eigen::Vector3d& vt,
                 Eigen::Quaterniond& swing, Eigen::Quaterniond& twist) {
    Eigen::Vector3d p = vt * (q.x() * vt[0] + q.y() * vt[1] + q.z() * vt[2]);
    twist = Eigen::Quaterniond(q.w(), p[0], p[1], p[2]);
    twist.normalize();
    swing = q * twist.conjugate();
}

// Behavior is undefined if q is not a unit quaternion.
// uses body 3-2-1 convention (z, y, x; heading, pitch, roll)
// see: NASA Mission Planning and Analysis Division. "Euler Angles, quaternions, and transformation matrices".
// also see: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
// their definitions of 'heading, attitude, and bank' are different from ours, but the equations still work
// because their coordinate axes are just our coordinate axes rotated a bit, i.e., the relationships between
// the axes are the same. Their heading = our pitch [y], attitude = heading [z], bank = roll [x].
// euler[0] is the angle of rotation in radians around the x-axis, in the range [-pi, pi].
// euler[1] " y-axis, in the range [-pi/2, pi/2]
// euler[2] " z-axis, in the range [-pi, pi]
Eigen::Vector3d quat_to_euler(Eigen::Quaterniond q)
{
    Eigen::Vector3d euler;

    // [q0 q1 q2 q3] is in w, x, y, z order
    const double q0 = q.w();
    const double q1 = q.x();
    const double q2 = q.y();
    const double q3 = q.z();

    euler[0] = atan2(2*(q0*q1 + q2*q3), 1-2*(q1*q1 + q2*q2));
    euler[1] = asin(2*(q0*q2 - q3*q1));
    euler[2] = atan2(2*(q0*q3 + q1*q2), 1-2*(q2*q2 + q3*q3));

    // Tentatively handle singularities.
    if (euler[1] > singularity_cutoff || euler[1] < -singularity_cutoff) {
        euler[0] = atan2(q3, q0);
        euler[2] = 0;
    }

    // XX this makes the controller angry
    //euler[0] = fmod(euler[0] + 2*M_PI, 2*M_PI);
    //euler[1] = fmod(euler[1] + 2*M_PI, 2*M_PI);
    //euler[2] = fmod(euler[2] + 2*M_PI, 2*M_PI);

    return euler;
}

// body 3-2-1
Eigen::Quaterniond euler_to_quat(double h, double p, double r)
{
    return Eigen::Quaterniond(Eigen::AngleAxisd(h, Eigen::Vector3d::UnitZ())
                            * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
                            * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()));
}

} // namespace fishbowl
} // namespace cuauv


#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;

};


#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) 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


//  (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;
}


#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;
	}
}


#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;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <thread>
#include <cmath>
#include <chrono>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#include "constants.h"
#include "cuda_wrapper.h"
#include "kernels.cuh"
#include "plot.h"

using namespace cv;

/**
 * Dataset from:
 *
 *   http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip
 *
 * */

inline double getBilinearInterpolatedValue_no_eigen(const unsigned char *img, double pt[2]) {
    const unsigned char* d = &img[(int)pt[1] * width + (int)pt[0]];
    double xx = pt[0] - floor(pt[0]);
    double yy = pt[1] - floor(pt[1]);
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[width]) +
            xx * yy * double(d[width + 1])) / 255.0;
}

inline void pix2cam_no_eigen(const double in[2], double out[3]) {
    out[0] = (in[0] - cx) / fx;
    out[1] =  (in[1] - cy) / fy;
    out[2] = 1.0;
}

inline void cam2pix_no_eigen(const double in[3], double out[2]) {
    out[0] = in[0] * fx / in[2] + cx;
    out[1] = in[1] * fy / in[2] + cy;
}

inline double norm3_no_eigen(const double in[3])
{
    return sqrt(in[0]*in[0] + in[1]*in[1] + in[2]*in[2]);
}

inline double norm2_no_eigen(const double in[2])
{
    return sqrt(in[0]*in[0] + in[1]*in[1]);
}

// inplace normalization vec 3

inline void normalize3_no_eigen(double in_out[3]) {
    double d = sqrt(in_out[0]*in_out[0]
                    + in_out[1]*in_out[1]
                    + in_out[2]*in_out[2]);
    in_out[0] /= d;
    in_out[1] /= d;
    in_out[2] /= d;
}

// inplace normalization vec 2

inline void normalize2_no_eigen(double in_out[2]) {
    double d = sqrt(in_out[0]*in_out[0] + in_out[1]*in_out[1]);
    in_out[0] /= d;
    in_out[1] /= d;
}

inline void transform_no_eigen(double x[3], const double T[12], double out[3])
{
    for (int i = 0; i < 3; ++i)
    {
        out[i] = x[0] * T[i*4] + x[1] * T[i*4+1] + x[2] * T[i*4+2] +  T[i*4+3];
    }
}

double ZNCC_no_eigen(const unsigned char *im1, const double pt1[2], const unsigned char *im2, const double pt2[2])
{
    // no need to consider block partly outside because of boarder
    double v1[ncc_area], v2[ncc_area];
    double s1 = 0.0, s2 = 0.0;
    int idx = 0;
    for (int i = -ncc_window_size; i <= ncc_window_size; ++i)
    {
        for (int j = -ncc_window_size; j <= ncc_window_size; ++j)
        {
            double val_1 = ((double) im1[((int)pt1[1] + i) * width + (int)pt1[0] + j]) / 255;
            double temp_p2[2] = {pt2[0] + j, pt2[1] + i};
            double val_2 = getBilinearInterpolatedValue_no_eigen(im2, temp_p2);
            s1 += val_1;
            s2 += val_2;
            v1[idx] = val_1;
            v2[idx] = val_2;
            ++idx;
        }
    }

    double mean_1 = s1 / ncc_area;
    double mean_2 = s2 / ncc_area;

    double numerator = 0.0;
    double den1 = 0.0, den2 = 0.0;
    for (int i = 0; i < ncc_area; ++i)
    {
        double zv1 = v1[i] - mean_1;
        double zv2 = v2[i] - mean_2;
        numerator += zv1*zv2;
        den1 += zv1 * zv1;
        den2 += zv2 * zv2;
    }
    auto zncc =  numerator / (sqrt(den1 * den2 + epsilon));
    // std::cout << "zncc = " << zncc << "\n";
    return zncc;
}

bool epipolar_search_no_eigen(const unsigned char* ref, const unsigned char* cur,
                          const double Tcr[12], const double pt[2],
                          double depth_mu, double depth_sigma2,
                          double best_pc[2], double epipolar_dir[2])
{

    double depth_sigma = sqrt(depth_sigma2);
    double dmax = depth_mu + 3 * depth_sigma;
    double dmin = depth_mu - 3 * depth_sigma;
    dmin = max(0.1, dmin);

    double pn[3];
    pix2cam_no_eigen(pt, pn);
    normalize3_no_eigen(pn);
    double P_max[3] = {pn[0] * dmax, pn[1] * dmax, pn[2] * dmax};
    double P_min[3] = {pn[0] * dmin, pn[1] * dmin, pn[2] * dmin};
    double P_mu[3] = {pn[0] * depth_mu, pn[1] * depth_mu, pn[2] * depth_mu};

    double P_max_cur[3], P_min_cur[3], P_mu_cur[3];
    transform_no_eigen(P_max, Tcr, P_max_cur);
    transform_no_eigen(P_min, Tcr, P_min_cur);
    transform_no_eigen(P_mu, Tcr, P_mu_cur);

    double pc_max[2], pc_min[2], pc_mu[2];
    cam2pix_no_eigen(P_max_cur, pc_max);
    cam2pix_no_eigen(P_min_cur, pc_min);
    cam2pix_no_eigen(P_mu_cur, pc_mu);

    double epipolar_line[2] = {pc_max[0] - pc_min[0], pc_max[1] - pc_min[1]};
    epipolar_dir[0] = epipolar_line[0];
    epipolar_dir[1] = epipolar_line[1];
    normalize2_no_eigen(epipolar_dir);
    double epipolar_line_norm = norm2_no_eigen(epipolar_line);

    // double step = 0.7;
    // int nb_samples = std::ceil(epipolar_line.norm() / step);

    double half_range = 0.5 * epipolar_line_norm;
    if (half_range > 100) half_range = 100;

    double best_zncc = -1.0;
    for (double l = -half_range; l<= half_range; l+= 0.7)
    {
        double p[2] = {pc_mu[0] + l * epipolar_dir[0], pc_mu[1] + l * epipolar_dir[1]};

        if (p[0] < boarder || p[0] >= width-boarder || p[1] < boarder || p[1] >= height-boarder)
            continue; // p is outside the cur image

        double zncc = ZNCC_no_eigen(ref, pt, cur, p);
        if (zncc > best_zncc)
        {
            best_zncc = zncc;
            best_pc[0] = p[0];
            best_pc[1] = p[1];
        }
    }
    if (best_zncc < 0.85)
        return false;
    else
        return true;
}

double dot3_no_eigen(const double a[3], const double b[3])
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

double det2_no_eigen(const double A[2][2])
{
    return A[0][0] * A[1][1] - A[1][0] * A[0][1];
}

void solve_Axb2_no_eigen(const double A[2][2], const double b[2], double res[2])
{
    double det_inv = 1.0 / det2_no_eigen(A);
    double A_inv[2][2];
    A_inv[0][0] = det_inv * A[1][1];
    A_inv[0][1] = -det_inv * A[0][1];
    A_inv[1][0] = -det_inv * A[1][0];
    A_inv[1][1] = det_inv * A[0][0];

    res[0] = A_inv[0][0] * b[0] + A_inv[0][1] * b[1];
    res[1] = A_inv[1][0] * b[0] + A_inv[1][1] * b[1];
}

void update_depth_filter_no_eigen(const double pr[2], const double pc[2], const double Trc[12], const double epipolar_dir[2], double *depth, double *cov2)
{
    double fr[3];
    pix2cam_no_eigen(pr, fr);
    normalize3_no_eigen(fr);

    double fc[3];
    pix2cam_no_eigen(pc, fc);
    normalize3_no_eigen(fc);

    double f2[3] = {dot3_no_eigen(Trc, fc),
                    dot3_no_eigen(Trc+4, fc),
                    dot3_no_eigen(Trc+8, fc)};

    double trc[3] = {Trc[3], Trc[7], Trc[11]};
    double A[2][2];
    double b[2];

    A[0][0] = dot3_no_eigen(fr, fr);
    A[0][1] = dot3_no_eigen(fr, f2);
    A[1][0] = dot3_no_eigen(f2, fr);
    A[1][1] = dot3_no_eigen(f2, f2);
    A[0][1] *= -1;
    A[1][1] *= -1;

    b[0] = dot3_no_eigen(fr, trc);
    b[1] = dot3_no_eigen(f2, trc);

    if (abs(det2_no_eigen(A)) < 1e-20) // not invertible
        return;

    double res[2];
    solve_Axb2_no_eigen(A, b, res);
    double P1[3] = {fr[0] * res[0], fr[1] * res[0], fr[2] * res[0]};
    double P2[3] = {trc[0] + fc[0] * res[1], trc[1] + fc[1] * res[1], trc[2] + fc[2] * res[1]};
    double P_est[3] = {(P1[0] + P2[0]) * 0.5,
                       (P1[1] + P2[1]) * 0.5,
                       (P1[2] + P2[2]) * 0.5};
    double depth_obs = norm3_no_eigen(P_est);

    double P[3] = {fr[0] * depth_obs, fr[1] * depth_obs, fr[2] * depth_obs};
    double a[3] = {P[0] - trc[0], P[1] - trc[1], P[2] - trc[2]};

    double t[3] = {trc[0], trc[1], trc[2]};
    normalize3_no_eigen(t);

    double alpha = acos(dot3_no_eigen(fr, t));
    double beta = acos(-dot3_no_eigen(a, t) / norm3_no_eigen(a));

    double pc2[2] = {pc[0] + epipolar_dir[0], pc[1] + epipolar_dir[1]};
    double fc2[3];
    pix2cam_no_eigen(pc2, fc2);
    normalize3_no_eigen(fc2);
    double beta_2 = acos(-dot3_no_eigen(fc2, t));

    double gamma = M_PI - alpha - beta_2;
    double d_noise = norm3_no_eigen(trc) * sin(beta_2) / sin(gamma); // sinus law
    double sigma_obs = depth_obs - d_noise;
    double sigma2_obs = sigma_obs * sigma_obs;

    // Depth fusion
    double d = depth[(int)pr[1] * width + (int)pr[0]];
    double sigma2 = cov2[(int)pr[1] * width + (int)pr[0]];

    double d_fused = (sigma2_obs * d + sigma2 * depth_obs) / (sigma2 + sigma2_obs);
    double sigma2_fused = (sigma2 * sigma2_obs) / (sigma2 + sigma2_obs);

    depth[(int)pr[1] * width + (int)pr[0]] = d_fused;
    cov2[(int)pr[1] * width + (int)pr[0]] = sigma2_fused;

}

void update_no_eigen(cv::Mat ref, cv::Mat cur, const Sophus::SE3d& Tcr, cv::Mat depth, cv::Mat cov2)
{
    Eigen::Vector2d pc;
    Eigen::Vector2d epipolar_dir;
    double pc_out[3];
    double epipolar_dir_out[2];

    Sophus::SE3d Trc = Tcr.inverse();
    double Tcr_data[12];
    double Trc_data[12];

    Eigen::Matrix<double, 3, 4, Eigen::RowMajor> Tcr_matrix = Tcr.matrix3x4();
    Eigen::Matrix<double, 3, 4, Eigen::RowMajor> Trc_matrix = Trc.matrix3x4();

    int total=0;
    for (int j = boarder; j < width-boarder; ++j)
    {
        for (int i = boarder; i < height-boarder; ++i)
        {
            double depth_mu = depth.at<double>(i, j);
            double depth_sigma2 = cov2.at<double>(i, j);
            if (depth_sigma2 < min_cov || depth_sigma2 > max_cov)
                continue;
            Eigen::Vector2d pr(j, i);
            bool found = epipolar_search_no_eigen(ref.ptr<unsigned char>(0), cur.ptr<unsigned char>(0),
                                                  Tcr_matrix.data(), pr.data(),
                                                  depth_mu, depth_sigma2,
                                                  pc.data(), epipolar_dir.data());
            total += found;
            if (!found)
                continue;
            // showEpipolarMatch(ref, cur, pr, pc);

            update_depth_filter_no_eigen(pr.data(), pc.data(), Trc_matrix.data(), epipolar_dir.data(), depth.ptr<double>(0), cov2.ptr<double>(0));
        }
    }
    std::cout << "total found " << total << " / " << width*height << "\n";
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // Read dataset
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // Initial depth image
    Mat ref = imread(color_image_files[0], 0); // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;
    double init_cov2 = 3.0;
    Mat depth(height, width, CV_64F, init_depth);
    Mat depth_cov2(height, width, CV_64F, init_cov2);

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // T_C_W * T_W_R = T_C_R
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        update_no_eigen(ref, curr, pose_T_C_R, depth, depth_cov2);
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

        auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
        std::cout << "Time used: " << time_used.count() << "s\n";

        evaludateDepth(ref_depth, depth);
        plotDepth(ref_depth, depth);
        plotCur(curr);
        // imshow("image", curr);
        // waitKey(1);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;

    return 0;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    std::vector<SE3d> &poses,
    cv::Mat &ref_depth) {
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while (!fin.eof()) {
        // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin >> image;
        double data[7];
        for (double &d:data) fin >> d;

        color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(
            SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if (!fin.good()) break;
    }
    fin.close();

    // load reference depth
    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if (!fin) return false;
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++) {
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }

    return true;
}

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    double ave_depth_error = 0;
    double ave_depth_error_sq = 0;
    int cnt_depth_data = 0;
    for (int y = boarder; y < depth_truth.rows - boarder; y++)
        for (int x = boarder; x < depth_truth.cols - boarder; x++) {
            double error = depth_truth.ptr<double>(y)[x] - depth_estimate.ptr<double>(y)[x];
            ave_depth_error += error;
            ave_depth_error_sq += error * error;
            cnt_depth_data++;
        }
    ave_depth_error /= cnt_depth_data;
    ave_depth_error_sq /= cnt_depth_data;

    cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}


/*
 // solvers for Algebraic Riccati equation
 // - Iteration (continuous)
 // - Iteration (discrete)
 // - Arimoto-Potter
 //
 // author: Horibe Takamasa
 */

#include <Eigen/Dense>
#include <iostream>
#include <time.h>
#include <vector>
#include "riccati_solver.h"

#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl

int
main ()
{
  const uint dim_x = 4;
  const uint dim_u = 1;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(dim_x, dim_u);
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_u, dim_u);
  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x, dim_x);

  A(0, 1) = 1.0;
  A(1, 1) = -15.0;
  A(1, 2) = 10.0;
  A(2, 3) = 1.0;
  A(3, 3) = -15.0;
  B(1, 0) = 10.0;
  B(3, 0) = 1.0;

  Q(0, 0) = 1.0;
  Q(2, 2) = 1.0;
  Q(3, 3) = 2.0;

  R(0, 0) = 1.0;

  PRINT_MAT(A);
  PRINT_MAT(B);
  PRINT_MAT(Q);
  PRINT_MAT(R);

  /* == iteration based Riccati solution (continuous) == */
  std::cout << "-- Iteration based method (continuous) --" << std::endl;
  clock_t start = clock();
  solveRiccatiIterationC(A, B, Q, R, P);
  clock_t end = clock();
  std::cout << "computation time = " << (double) (end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
  PRINT_MAT(P);

  /* == iteration based Riccati solution (discrete) == */
  // discretization
  const double dt = 0.001;
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x, dim_x);
  Eigen::MatrixXd Ad = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Ad = (I + 0.5 * dt * A) * (I - 0.5 * dt * A).inverse();
  Eigen::MatrixXd Bd;
  Bd = B * dt;

  std::cout << "-- Iteration based method (discrete)--" << std::endl;
  start = clock();
  solveRiccatiIterationD(Ad, Bd, Q, R, P);
  end = clock();
  std::cout << "computation time = " << (double) (end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
  PRINT_MAT(P);

  /* == eigen decomposition method (Arimoto-Potter algorithm) == */
  std::cout << "-- Eigen decomposition mathod --" << std::endl;
  start = clock();
  solveRiccatiArimotoPotter(A, B, Q, R, P);
  end = clock();
  std::cout << "computation time = " << (double) (end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
  PRINT_MAT(P);

  return 0;
}


/*
* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
* its licensors.
*
* For complete copyright and license terms please see the LICENSE at the root of this
* distribution (the "License"). All use of this software is governed by the License,
* or, if provided, by the license below or the license accompanying this file. Do not
* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*
*/

#include <NumericalMethods_precompiled.h>

#include <cmath>

#include <AzCore/std/algorithm.h>

#include <LinearAlgebra.h>
#include <Eigenanalysis/Utilities.h>

namespace NumericalMethods::Eigenanalysis
{
    VectorVariable CrossProduct(const VectorVariable& lhs, const VectorVariable& rhs)
    {
        AZ_Assert(
            lhs.GetDimension() == 3 && rhs.GetDimension() == 3, "VectorVariable dimensions invalid for cross product."
        );

        return VectorVariable::CreateFromVector({
            lhs[1] * rhs[2] - lhs[2] * rhs[1],
            lhs[2] * rhs[0] - lhs[0] * rhs[2],
            lhs[0] * rhs[1] - lhs[1] * rhs[0]
        });
    }

    void ComputeOrthogonalComplement(
        const VectorVariable& vecW, VectorVariable& vecU, VectorVariable& vecV
    )
    {
        // Robustly computes a right-handed orthogonal basis {vecU, vecV, vecW}.
        double invLength = 1.0;

        if (fabs(vecW[0]) > fabs(vecW[1]))
        {
            // The component of maximum absolute value is either vecW[0] or vecW[2].
            invLength /= sqrt(vecW[0] * vecW[0] + vecW[2] * vecW[2]);
            vecU = VectorVariable::CreateFromVector({ -vecW[2] * invLength, 0.0, vecW[0] * invLength });
        }
        else
        {
            // The component of maximum absolute value is either vecW[1] or vecW[2].
            invLength /= sqrt(vecW[1] * vecW[1] + vecW[2] * vecW[2]);
            vecU = VectorVariable::CreateFromVector({ 0.0, vecW[2] * invLength, -vecW[1] * invLength });
        }

        vecV = CrossProduct(vecW, vecU);
    }

    VectorVariable ComputeEigenvector0(
        double a00, double a01, double a02, double a11, double a12, double a22, double val
    )
    {
        // By definition, (A−e∗I)v = 0, where e is the eigenvalue and v is the corresponding eigenvector to be found.
        // This condition implies that the rows (A−e∗I) must be perpendicular to v. This matrix must have rank 2, so two
        // rows will be linearly dependent. For those two rows, the cross product will be (nearly) zero. So to find v,
        // we can simply take the cross product of the two rows that maximize its magnitude.
        VectorVariable row0 = VectorVariable::CreateFromVector({ a00 - val, a01, a02 });
        VectorVariable row1 = VectorVariable::CreateFromVector({ a01, a11 - val, a12 });
        VectorVariable row2 = VectorVariable::CreateFromVector({ a02, a12, a22 - val });

        VectorVariable r0xr1 = CrossProduct(row0, row1);
        VectorVariable r0xr2 = CrossProduct(row0, row2);
        VectorVariable r1xr2 = CrossProduct(row1, row2);

        double d0 = r0xr1.Dot(r0xr1);
        double d1 = r0xr2.Dot(r0xr2);
        double d2 = r1xr2.Dot(r1xr2);

        return d0 >= d1 && d0 >= d2 ? r0xr1 * (1.0 / sqrt(d0)) :
               d1 >= d0 && d1 >= d2 ? r0xr2 * (1.0 / sqrt(d1)) :
                                      r1xr2 * (1.0 / sqrt(d2)) ;
    }

    VectorVariable ComputeEigenvector1(
        double a00,
        double a01,
        double a02,
        double a11,
        double a12,
        double a22,
        double val,
        const VectorVariable& vec
    )
    {
        // Real symmetric matrices must have orthogonal eigenvectors. Thus, if we generate two vectors vecU and vecV
        // orthogonal to the eigenvector vec already found, the remaining eigenvectors must be a circular combination
        // of vecU and vecW. This reduces the problem to a 2D system. For details see Eberly.
        VectorVariable vecU(3);
        VectorVariable vecV(3);
        ComputeOrthogonalComplement(vec, vecU, vecV);

        MatrixVariable matA(3, 3);

        matA.Element(0, 0) = a00;
        matA.Element(0, 1) = a01;
        matA.Element(0, 2) = a02;

        matA.Element(1, 0) = a01;
        matA.Element(1, 1) = a11;
        matA.Element(1, 2) = a12;

        matA.Element(2, 0) = a02;
        matA.Element(2, 1) = a12;
        matA.Element(2, 2) = a22;

        double m00 = vecU.Dot(matA * vecU) - val;
        double absM00 = fabs(m00);

        double m01 = vecU.Dot(matA * vecV);
        double absM01 = fabs(m01);

        double m11 = vecV.Dot(matA * vecV) - val;
        double absM11 = fabs(m11);

        auto discardComponentAndNormalize = [](double& factor, double& other) {
            other /= factor;
            factor = 1.0 / sqrt(1.0 + other * other);
            other *= factor;
        };

        if (absM00 > absM11)
        {
            if (AZStd::max(absM00, absM01) > 0.0)
            {
                if (absM00 >= absM01)
                {
                    discardComponentAndNormalize(m00, m01);
                }
                else
                {
                    discardComponentAndNormalize(m01, m00);
                }
                return vecU * m01 - vecV * m00;
            }
            else
            {
                return vecU;
            }
        }
        else
        {
            if (AZStd::max(absM11, absM01) > 0.0)
            {
                if (absM11 >= absM01)
                {
                    discardComponentAndNormalize(m11, m01);
                }
                else
                {
                    discardComponentAndNormalize(m01, m11);
                }
                return vecU * m11 - vecV * m01;
            }
            else
            {
                return vecU;
            }
        }
    }

    VectorVariable ComputeEigenvector2(const VectorVariable& vec0, const VectorVariable& vec1)
    {
        return CrossProduct(vec0, vec1);
    }
} // namespace NumericalMethods::Eigenanalysis


/** @file NormalDist.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief NormalDist class definition
 *
 */
#include "PriorHessian/NormalDist.h"
#include "PriorHessian/PriorHessianError.h"

#include <sstream>
#include <cmath>
#include <limits>

#include <boost/math/special_functions/erf.hpp>
#include <boost/math/constants/constants.hpp>

namespace prior_hessian {

/* Static member variables */
const StringVecT NormalDist::_param_names = { "mu", "sigma" };
const NormalDist::NparamsVecT NormalDist::_param_lbound = {-INFINITY, 0}; //Lower bound on valid parameter values
const NormalDist::NparamsVecT NormalDist::_param_ubound = {INFINITY, INFINITY}; //Upper bound on valid parameter values

/* Constructors */
NormalDist::NormalDist(double mu, double sigma)
    : UnivariateDist()
{
    set_params(mu,sigma);
}

/* Non-static member functions */

void NormalDist::set_sigma(double val)
{
    _sigma = checked_sigma(val);
    _sigma_inv = 1./_sigma;
    llh_const_initialized = false;
}

double NormalDist::cdf(double x) const
{
    return .5*(1 + boost::math::erf((x - _mu)*_sigma_inv*constants::sqrt2_inv));
}

double NormalDist::icdf(double u) const
{
    return mu() + sigma()*constants::sqrt2*boost::math::erf_inv(2*u-1);
}

double NormalDist::checked_mu(double val)
{
    if(!std::isfinite(val)) {
        std::ostringstream msg;
        msg<<"NormalDist: got bad mu value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

double NormalDist::checked_sigma(double val)
{
    if(val<=0 || !std::isfinite(val)) {
        std::ostringstream msg;
        msg<<"NormalDist: got bad sigma value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

double NormalDist::llh(double x) const
{
    if(!llh_const_initialized) initialize_llh_const(); //Lazy computation of llh_const.
    return rllh(x) + llh_const;
}

void NormalDist::initialize_llh_const() const
{
    llh_const = compute_llh_const(sigma());
    llh_const_initialized = true;
}

double NormalDist::compute_llh_const(double sigma)
{
    return -log(sigma) - .5*constants::log2pi;
}

} /* namespace prior_hessian */