/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <algorithm>
#include <random>

#include <iostream>

#define LOG_2PI 1.8378770664093453

template<typename T, int D>
class Normal
{
 public:

  Normal();
  Normal(const Eigen::Matrix<T,D,1>& mu, const Eigen::Matrix<T,D,D>& Sigma);
  Normal(const Eigen::Matrix<T,D,D>& Sigma);
  Normal(const Normal<T,D>& other);
  ~Normal();

  T logPdf(const Eigen::Matrix<T,D,D>& x) const;
  T logPdfSlower(const Eigen::Matrix<T,D,D>& x) const;
  T logPdf(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const;
  T logPdfSlower(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const;
  T logPdf(const Eigen::Matrix<T,D,D>& scatter, T count) const;

  Eigen::Matrix<T,D,1> sample(std::mt19937& rnd);

  void print() const;

  const Eigen::Matrix<T,D,D>& Sigma() const {return Sigma_;};
  void setSigma(const Eigen::Matrix<T,D,D>& Sigma)
  { Sigma_ = Sigma; SigmaLDLT_.compute(Sigma_);
    logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();};
  T logDetSigma() const {return logDetSigma_;};
  const Eigen::LDLT<Eigen::Matrix<T,D,D> >& SigmaLDLT() const {return SigmaLDLT_;};

  Eigen::Matrix<T,D,1> mu_;
  Eigen::Matrix<T,D,D> Sigma_;
private:

  // helpers for fast computation
  T logDetSigma_;
  Eigen::LDLT<Eigen::Matrix<T,D,D> > SigmaLDLT_;

  std::normal_distribution<T> gauss_;
};

template<typename T, int D>
Normal<T,D>::Normal(const Eigen::Matrix<T,D,1>& mu, const Eigen::Matrix<T,D,D>& Sigma)
  : mu_(mu), Sigma_(Sigma), SigmaLDLT_(Sigma_), gauss_(0,1)
{
  // equivalent to log(det(Sigma)) but more stable for small values
  logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();
};

template<typename T, int D>
Normal<T,D>::Normal(const Eigen::Matrix<T,D,D>& Sigma)
  : mu_(Eigen::Matrix<T,D,1>::Zero()), Sigma_(Sigma), SigmaLDLT_(Sigma_) , gauss_(0,1)
{
  // equivalent to log(det(Sigma)) but more stable for small values
  logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();
};

template<typename T, int D>
Normal<T,D>::Normal()
  : mu_(Eigen::Matrix<T,D,1>::Zero()), Sigma_(Eigen::Matrix<T,D,D>::Identity()), SigmaLDLT_(Sigma_), gauss_(0,1)
{
  // equivalent to log(det(Sigma)) but more stable for small values
  logDetSigma_ = ((Sigma_.eigenvalues()).array().log().sum()).real();
};

template<typename T, int D>
Normal<T,D>::Normal(const Normal<T,D>& other)
  : mu_(other.mu_), Sigma_(other.Sigma_),
  logDetSigma_(other.logDetSigma_), SigmaLDLT_(Sigma_) , gauss_(0,1)
{};

template<typename T, int D>
Normal<T,D>::~Normal()
{}

template<typename T, int D>
T Normal<T,D>::logPdf(const Eigen::Matrix<T,D,D>& x) const
{
  return -0.5*(LOG_2PI*D + logDetSigma_ +((x-mu_).transpose()*SigmaLDLT_.solve(x-mu_)).sum() );
}

template<typename T, int D>
T Normal<T,D>::logPdfSlower(const Eigen::Matrix<T,D,D>& x) const
{
  return -0.5*(LOG_2PI*D + logDetSigma_
//      +((x-mu_).transpose()*Sigma_.inverse()*(x-mu_)).sum() );
  +((x-mu_).transpose()*Sigma_.fullPivHouseholderQr().solve(x-mu_)).sum() );
}

template<typename T, int D>
T Normal<T,D>::logPdf(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const
{
	Eigen::Matrix<T,1,D> meanT = mean.transpose();
	Eigen::Matrix<T,D,D> CovMu= SigmaLDLT_.solve(mu_);
	return -0.5*((LOG_2PI*D + logDetSigma_)*count
		+ count*(mu_.transpose()*CovMu).sum()
		-2.*count*(meanT*CovMu).sum()
		+(SigmaLDLT_.solve(scatter + mean*meanT*count)).trace());

  //return -0.5*((LOG_2PI*D + logDetSigma_)*count
  //    + count*(mu_.transpose()*SigmaLDLT_.solve(mu_)).sum()
  //    -2.*count*(mean.transpose()*SigmaLDLT_.solve(mu_)).sum()
  //    +(SigmaLDLT_.solve(scatter + mean*mean.transpose()*count )).trace());
}

template<typename T, int D>
T Normal<T,D>::logPdfSlower(const Eigen::Matrix<T,D,D>& scatter,
      const Eigen::Matrix<T,D,1>& mean, T count) const
{
	Eigen::Matrix<T,1,D> meanT = mean.transpose();
	Eigen::Matrix<T,D,D> CovMu=
    Sigma_.fullPivHouseholderQr().solve(mu_);
	return -0.5*((LOG_2PI*D + logDetSigma_)*count
		+ count*(mu_.transpose()*CovMu).sum()
		-2.*count*(meanT*CovMu).sum()
		+(Sigma_.fullPivHouseholderQr().solve(
      scatter + mean*meanT*count)).trace());
}

template<typename T, int D>
T Normal<T,D>::logPdf(const Eigen::Matrix<T,D,D>& scatter, T count) const
{
  assert(false);
//  cout<<count*(mu_.transpose()*SigmaLDLT_.solve(mu_)).sum()<<endl;
//  cout<<(SigmaLDLT_.solve(scatter)).trace()<<endl;
  return -0.5*((LOG_2PI*D + logDetSigma_)*count
      + count*(mu_.transpose()*SigmaLDLT_.solve(mu_)).sum()
      +(SigmaLDLT_.solve(scatter)).trace());
}

template<typename T, int D>
Eigen::Matrix<T,D,1> Normal<T,D>::sample(std::mt19937& rnd)
{
  // populate the mean
  Eigen::Matrix<T,D,1> x(D);
  for (uint32_t d=0; d<D; d++)
    x[d] = gauss_(rnd); //gsl_ran_gaussian(r,1);
  Eigen::Matrix<T,D,D> sqrtD = Eigen::Matrix<T,D,D>::Zero(D,D);
  sqrtD.diagonal() = SigmaLDLT_.vectorD();
  sqrtD = sqrtD.array().sqrt();
  return (SigmaLDLT_.matrixL()*sqrtD*SigmaLDLT_.matrixU())*x + mu_;
};


/*!
 * @file
 * @brief
 * @copyright alphya 2018-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_DIFF_HPP
#define NYARUGA_UTIL_DIFF_HPP

#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cmath>
#include <limits>
#include <nyaruga_util/get_arg_type.hpp>
#include <type_traits>

namespace nyaruga {

namespace util {

using num_t =
   boost::multiprecision::number<boost::multiprecision::cpp_dec_float<1000>>;

// 微分します
// 7 点近似です
template <typename Number = num_t, typename F>
constexpr auto diff(F && f) noexcept
{
   return [f](auto && x) noexcept(noexcept(f(x))) -> Number {
      auto h = 1e-7;
      if constexpr (std::is_same_v<Number, num_t>)
         h = DBL_EPSILON;

      auto&& y1 = f(static_cast<Number>(x + h));
      auto&& y2 = f(static_cast<Number>(x - h));
      auto&& y3 = f(static_cast<Number>(x + 2 * h));
      auto&& y4 = f(static_cast<Number>(x - 2 * h));
      auto&& y5 = f(static_cast<Number>(x + 3 * h));
      auto&& y6 = f(static_cast<Number>(x - 3 * h));

      return (y5 - 9 * y3 + 45 * y1 - 45 * y2 + 9 * y4 - y6) /
                           (60 * h);
   };
}

// 5 点近似です
template <typename Number = num_t, typename F>
constexpr auto diff_fast(F && f) noexcept
{
   return [f](auto && x) noexcept(noexcept(f(x))) -> Number
   {
      auto h = 1e-7;
      if constexpr (std::is_same_v<Number, num_t>)
         h = DBL_EPSILON;

      auto && y1 = f(static_cast<Number>(x + h));
      auto && y2 = f(static_cast<Number>(x - h));
      auto && y3 = f(static_cast<Number>(x + 2 * h));
      auto && y4 = f(static_cast<Number>(x - 2 * h));

      return (y4 - 8 * y2 + 8 * y1 - y3) / (12 * h);
   };
}

} // namespace util

} // namespace nyaruga

#endif // #ifndef NYARUGA_UTIL_DIFF_HPP


#pragma once
#ifndef _MATHUTIL_H
#define _MATHUTIL_H

#include <assert.h>

#include <random>
#include <limits>
#include <memory>
#include <iostream>
#include <iomanip>

#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

namespace mut
{
   const double PI = 3.14159265358979323846;
   const float PI_F = 3.14159265358979f;
   const double NaN = std::numeric_limits<double>::quiet_NaN();
   const double NaN_F = std::numeric_limits<float>::quiet_NaN();

   static Eigen::Quaterniond rotate_between(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                            const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

   template<typename T> static inline bool near_zero(T v, T epsilon);

   template<> bool near_zero(long double v, long double epsilon) { return (fabsl(v) <= epsilon); }

   template<> bool near_zero(double v, double epsilon) { return (fabs(v) <= epsilon); }

   template<> bool near_zero(float v, float epsilon) { return (fabsf(v) <= epsilon); }

   inline static double radiansToDegrees(double radians) { return (radians * 180.0 / PI); }

   inline static float radiansToDegrees(float radians) { return (radians * 180.0f / PI_F); }

   inline static double degreesToRadians(double degrees) { return (degrees * PI / 180.0); }

   inline static float degreesToRadians(float degrees) { return (degrees * PI_F / 180.0f); }

   template<typename T>
   inline static void normalize(T &v)
//-----------------------------------------------
   {
      auto magnitude = v.dot(v);
      if (magnitude > 1.0e-8)
         v /= sqrt(magnitude);
   }

   template<typename T>
   inline static void unchecked_normalize(T &v)
   { v /= sqrt(v.dot(v)); }

   inline static Eigen::Matrix3d skew33d(Eigen::Vector3d v3)
  //-----------------------------------------------
   {
      double a = v3[0], b = v3[1], c = v3[2];
      Eigen::Matrix3d X;
      X << 0, -c, b, c, 0, -a, -b, a, 0;
      return X;
   }

   inline static Eigen::Vector3d solve_homogeneous(Eigen::Matrix3d A)
//---------------------------------------------------------------------------------------------------------------------------
   {
//   std::cout << "Ax = 0:" << std::endl << A << std::endl;
      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
//   std::cout << "U: " << U << std::endl << "S: " << svd.singularValues() << std::endl << "V: " << V << std::endl;
      return V.col(V.cols() - 1);
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, double x, double y, double z = 1,
                                                bool is_homogenouous = false, bool is_normalize = false)
//----------------------------------------------------------------------------------
   {
      Eigen::Vector3d v(x, y, z);
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      return ray;
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, Eigen::Vector3d v, bool is_homogenouous = false,
                                                bool is_normalize = false)
//--------------------------------------------------------------------------------------------------
   {
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      //Eigen::Vector3d p  = KI.inverse()*Eigen::Vector3d(1128,592);
      return ray;
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, cv::Point3d pt, bool is_homogenouous = false,
                                                bool is_normalize = false)
   //-----------------------------------------------------------------------------------------------------------------------
   {
      Eigen::Vector3d v(pt.x, pt.y, pt.z);
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      return ray;
   }

   inline static void yawpitchroll(const Eigen::Vector3d train, const Eigen::Vector3d query,
                                   double& roll_degrees, double& pitch_degrees, double &yaw_degrees)
   //----------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond Q = rotate_between(query, train);
      Eigen::Matrix3d R = Q.toRotationMatrix();
      Eigen::Vector3d euler = R.eulerAngles(0, 1, 2);
      roll_degrees = radiansToDegrees(euler[0]);
      pitch_degrees = radiansToDegrees(euler[1]);
      yaw_degrees = radiansToDegrees(euler[2]);

      std::cout << std::fixed << std::setprecision(8) << train.transpose() << " -> "  << query.transpose()
                << ": Roll " << mut::radiansToDegrees(euler[0]) << " Pitch " << mut::radiansToDegrees(euler[1])
                << " Yaw " << mut::radiansToDegrees(euler[2]) << std::endl << R << std::endl;

      Q.setFromTwoVectors(query, train);
      R = Q.toRotationMatrix();
      euler = R.eulerAngles(0, 1, 2);
      std::cout << std::fixed << std::setprecision(8) << train.transpose() << " -> "  << query.transpose()
                << ": Roll " << mut::radiansToDegrees(euler[0]) << " Pitch " << mut::radiansToDegrees(euler[1])
                << " Yaw " << mut::radiansToDegrees(euler[2]) << std::endl << R << std::endl;
   }

   static Eigen::Vector3d quaternion_to_euler_angle(Eigen::Quaterniond Q)
   //--------------------------------------------------------------------
   {
      double w = Q.w();
      double x = Q.x();
      double y = Q.y();
      double z = Q.z();

      double ysqr = y * y;

      double t0 = +2.0 * (w * x + y * z);
      double t1 = +1.0 - 2.0 * (x * x + ysqr);
      double X = atan2(t0, t1);

      double t2 = +2.0 * (w * y - z * x);
      t2 = (t2 > +1.0) ? +1.0 : t2;
      t2 = (t2 < -1.0) ? -1.0 : t2;
      double Y = asin(t2);

      double t3 = +2.0 * (w * z + x * y);
      double t4 = +1.0 - 2.0 * (ysqr + z * z);
      double Z = atan2(t3, t4);

      return Eigen::Vector3d(X, Y, Z);
   }

   static cv::Vec3f rotation2Euler(cv::Mat &R)
   //-----------------------------------------
   {
      Eigen::Matrix3d ER;
      cv::cv2eigen(R, ER);
      Eigen::Vector3d euler = ER.eulerAngles(0, 1, 2);
      return cv::Vec3f(euler[0], euler[1], euler[2]);
   }

   static Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw )
   //-------------------------------------------------------------------------------------------
   {
      Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
      Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
      Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
      Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
      return q;
   }

   static cv::Vec3d ocvMultMatVec(const cv::Mat &P, double x, double y, double z)
   //---------------------------------------------------------------------------
   {
      cv::Mat V(1, 3, CV_64FC1);
      double *pV = V.ptr<double>(0);
      pV[0] = x;
      pV[1] = y;
      pV[2] = z;
      cv::Mat R = P * V.t();
      R = R.t();
      pV = R.ptr<double>(0);
      return cv::Vec3d(pV[0], pV[1], pV[2]);
   }

   static cv::Vec3d ocvMultMatVec(const cv::Mat &P, cv::Vec3d v)
   //------------------------------------------------------------
   {
      cv::Mat V(1, 3, CV_64FC1);
      double *pV = V.ptr<double>(0);
      pV[0] = v[0];
      pV[1] = v[1];
      pV[2] = v[2];
      cv::Mat R = P * V.t();
      R = R.t();
      pV = R.ptr<double>(0);
      return cv::Vec3d(pV[0], pV[1], pV[2]);
   }

   static inline cv::Vec3d toOcvRotationVec(Eigen::Quaterniond Q)
   //-----------------------------------------------------
   {
      Eigen::AngleAxisd aa(Q);
      Eigen::Vector3d v = aa.angle()*aa.axis();
/*
      auto R = Q.toRotationMatrix();
      cv::Mat OR;
      cv::eigen2cv(R, OR);
      cv::Vec3d vv;
      cv::Rodrigues(OR, vv);
      std::cout << v.transpose() << " " << vv.t() << std::endl;
*/
      return cv::Vec3d(v[0], v[1], v[2]);
   }

   static inline Eigen::Quaterniond toQuaternion(cv::InputArray m)
   //-----------------------------------------------------
   {
      cv::Mat R;
      cv::Rodrigues(m, R);
      Eigen::Matrix3d ER;
      cv::cv2eigen(R, ER);
      return Eigen::Quaterniond(ER);
   }

   static void decode_quaternion(const Eigen::Quaterniond qin, Eigen::Vector3d& axis, double& angle)
   //-------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond Q = qin;
      if (Q.w() > 1)
         Q.normalize(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
      angle = 2 * acos(Q.w());
      double s = sqrt(1 - Q.w()*Q.w()); // assuming quaternion normalised then w is less than 1, so term always positive.
      if (s >= 0.001)
      { // test to avoid divide by zero, s is always positive due to sqrt
         // if s close to zero then direction of axis not important
         axis[0] = Q.x(); // if it is important that axis is normalised then replace with x=1; y=z=0;
         axis[1] = Q.y();
         axis[2] = Q.z();
      }
      else
      {
         axis[0] = Q.x() / s;
         axis[1] = Q.y() / s;
         axis[2] = Q.z() / s;
         axis.normalize();
      }
   }

   static void essential_decompose(const Eigen::Matrix3d& essential_matrix, Eigen::Matrix3d* rotation1,
                            Eigen::Matrix3d* rotation2, Eigen::Vector3d* translation)
   //------------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d d;
      d << 0, 1, 0, -1, 0, 0, 0, 0, 1;

      const Eigen::JacobiSVD<Eigen::Matrix3d> svd(
            essential_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
      Eigen::Matrix3d U = svd.matrixU();
      Eigen::Matrix3d V = svd.matrixV();
      if (U.determinant() < 0) {
         U.col(2) *= -1.0;
      }

      if (V.determinant() < 0) {
         V.col(2) *= -1.0;
      }

      // Possible configurations.
      *rotation1 = U * d * V.transpose();
      *rotation2 = U * d.transpose() * V.transpose();
      *translation = U.col(2).normalized();
   }

   inline double essential_check(const Eigen::Matrix3d R, const double tx, const double ty, const double tz,
                                 const double xt, const double yt, const double xq,  const double yq)
   {
      return  -R(0, 0)*ty*xt - R(0, 1)*ty*yt - R(0, 2)*ty + R(1, 0)*tx*xt + R(1, 1)*tx*yt +
               R(1, 2)*tx + xq*(-R(1, 0)*tz*xt - R(1, 1)*tz*yt - R(1, 2)*tz + R(2, 0)*ty*xt +
               R(2, 1)*ty*yt + R(2, 2)*ty) - yq*(-R(0, 0)*tz*xt - R(0, 1)*tz*yt - R(0, 2)*tz +
               R(2, 0)*tx*xt + R(2, 1)*tx*yt + R(2, 2)*tx);
   }

   static bool check_essential(Eigen::Matrix3d R, Eigen::Vector3d T, double epsilon =0.0000001)
   //------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d E = skew33d(T)*R;
//      std::cout << R << std::endl << E << std::endl;
      if (! near_zero(E.determinant(), epsilon))
         return false;
      Eigen::Matrix3d EET = E*E.transpose();
      Eigen::Matrix3d constraint = 2*EET*E - EET.trace()*E;
//      std::cout << constraint << std::endl;
      struct ZeroVisit
      {
         double total = 0;
         void init(const double& value, Eigen::Index i, Eigen::Index j) {}
         void operator() (const double& value, Eigen::Index i, Eigen::Index j) { total += value; }
      } visitor;
      constraint.visit(visitor);
      if (! near_zero(visitor.total, epsilon))
         return false;

      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
      auto S = svd.singularValues().asDiagonal().toDenseMatrix();
//      std::cout << "U: " << U << std::endl << "S: " << S << std::endl << "V: " << V << std::endl;
      if (!near_zero(S(2, 2), epsilon))
         return false;
   //   if ( (!near_zero(S(0, 0) - 1, 0.0000001)) || (!near_zero(S(1, 1) - 1, 0.0000001)) )
   //   {
   //      S(0, 0) = S(1, 1) = 1.0;
   //      S(2, 2) = 0.0;
   //      auto A = U*S*V.transpose();
   //      std::cout << A << std::endl;
   //   }
      return true;
   }

   // or just R = Eigen::Quaterniond::UnitRandom().toRotationMatrix();
   static void randomRotationMatrix(std::mt19937* generator, std::uniform_real_distribution<double>* distribution,
                                    Eigen::Matrix3d& R)
//-------------------------------------------------------------------------------------------------------------
   {
      double x = (*distribution)(*generator);
      double y = (*distribution)(*generator);
      double z = (*distribution)(*generator);
      double theta = std::acos(2*x - 1);
      double phi = 2*mut::PI*y;

      double r  = sqrt(z);
      double Vx = sin(phi) * r;
      double Vy = cos(phi) * r;
      double Vz = sqrt(2.f - z);

      double st = sin(theta);
      double ct = cos(theta);
      double Sx = Vx * ct - Vy * st;
      double Sy = Vx * st + Vy * ct;

      R(0,0) = Vx * Sx - ct;
      R(0,1) = Vx * Sy - st;
      R(0,2) = Vx * Vz;
      R(1,0) = Vy * Sx + st;
      R(1,1) = Vy * Sy - ct;
      R(1,2) = Vy * Vz;
      R(2,0) = Vz * Sx;
      R(2,1) = Vz * Sy;
      R(2,2) = 1.0 - z;

      assert(mut::near_zero(R.determinant() - 1.0, 0.000001));
   }

   static void random_vector3d_fixed_norm_kludge(const double norm_sqrt, Eigen::Vector3d& v)
   //-------------------------------------------------------------------------------
   {
      std::random_device rand_device;
      std::mt19937 rand_generator;
      std::unique_ptr<std::uniform_real_distribution<double>> distribution;
      std::uniform_int_distribution<int> index_distr(0,2);
      std::vector<double> pot;

      distribution.reset(new std::uniform_real_distribution<double>(0, norm_sqrt));
      double one = ((*distribution)(rand_generator));
      distribution.reset(new std::uniform_real_distribution<double>(0, std::max(norm_sqrt - one, 0.0)));
      double two = ((*distribution)(rand_generator));
      double three = std::max(norm_sqrt - one - two, 0.0);
      pot.clear();
      pot.push_back(one); pot.push_back(two); pot.push_back(three);
      int i1 = index_distr(rand_generator), i2 = index_distr(rand_generator), i3;
      while (i2 == i1) i2 = index_distr(rand_generator);
      switch (i1)
      {
         case 0: if (i2 == 1) i3 = 2; else i3 = 1; break;
         case 1: if (i2 == 0) i3 = 2; else i3 = 0; break;
         case 2: if (i2 == 0) i3 = 1; else i3 = 0; break;
      }
      v[0] = pot[i1]; v[1] = pot[i2]; v[2] = pot[i3];
   }

/*
int essential_best_pose(const Eigen::Matrix3d& essential_matrix,
                        const std::vector<FeatureCorrespondence>& normalized_correspondences,
                        Eigen::Matrix3d* rotation, Eigen::Vector3d* position)
{
   Eigen::Matrix3d rotation1, rotation2;
   Eigen::Vector3d translation;
   essential_decompose(essential_matrix, &rotation1, &rotation2, &translation);
   const std::vector<Eigen::Matrix3d> rotations = {rotation1, rotation1, rotation2, rotation2};
   const std::vector<Eigen::Vector3d> positions = {
         -rotations[0].transpose() * translation,
         -rotations[1].transpose() * -translation,
         -rotations[2].transpose() * translation,
         -rotations[3].transpose() * -translation};

   // From the 4 candidate poses, find the one with the most triangulated points
   // in front of the camera.
   std::vector<int> points_in_front_of_cameras(4, 0);
   for (int i = 0; i < 4; i++) {
      for (const auto& correspondence : normalized_correspondences) {
         if (IsTriangulatedPointInFrontOfCameras(
               correspondence, rotations[i], positions[i])) {
            ++points_in_front_of_cameras[i];
         }
      }
   }

   // Find the pose with the most points in front of the camera.
   const auto& max_element = std::max_element(points_in_front_of_cameras.begin(),
                                              points_in_front_of_cameras.end());
   const int max_index =
         std::distance(points_in_front_of_cameras.begin(), max_element);

   // Set the pose.
   *rotation = rotations[max_index];
   *position = positions[max_index];
   return *max_element;
}
*/
}
#endif


#include <common_robotics_utilities/conversions.hpp>

#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace conversions
{
Eigen::Quaterniond QuaternionFromRPY(const double R,
                                     const double P,
                                     const double Y)
{
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(roll * pitch * yaw);
  return quat;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Quaterniond QuaternionFromUrdfRPY(const double R,
                                         const double P,
                                         const double Y)
{
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(yaw * pitch * roll);
  return quat;
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromRotationMatrix(
    const Eigen::Matrix3d& rot_matrix)
{
  // Use XYZ angles
  const Eigen::Vector3d euler_angles = rot_matrix.eulerAngles(0, 1, 2);
  return euler_angles;
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat)
{
  return EulerAnglesFromRotationMatrix(quat.toRotationMatrix());
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans)
{
  return EulerAnglesFromRotationMatrix(trans.rotation());
}

Eigen::Isometry3d TransformFromXYZRPY(const double x,
                                      const double y,
                                      const double z,
                                      const double roll,
                                      const double pitch,
                                      const double yaw)
{
  const Eigen::Isometry3d transform
      = Eigen::Translation3d(x, y, z) * QuaternionFromRPY(roll, pitch, yaw);
  return transform;
}

Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation,
                                   const Eigen::Vector3d& rotation)
{
  const Eigen::Isometry3d transform
      = static_cast<Eigen::Translation3d>(translation)
            * QuaternionFromRPY(rotation.x(), rotation.y(), rotation.z());
  return transform;
}

Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components)
{
  if (components.size() == 6)
  {
    return Eigen::Translation3d(components(0),
                                components(1),
                                components(2))
           * QuaternionFromRPY(components(3),
                               components(4),
                               components(5));
  }
  else
  {
    throw std::invalid_argument(
          "VectorXd source vector is not 6 elements in size");
  }
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x,
                                          const double y,
                                          const double z,
                                          const double roll,
                                          const double pitch,
                                          const double yaw)
{
  const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z)
                                      * QuaternionFromUrdfRPY(roll, pitch, yaw);
  return transform;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation,
                                       const Eigen::Vector3d& rotation)
{
  const Eigen::Isometry3d transform
      = static_cast<Eigen::Translation3d>(translation)
          * QuaternionFromUrdfRPY(rotation.x(), rotation.y(), rotation.z());
  return transform;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components)
{
  if (components.size() == 6)
  {
    return Eigen::Translation3d(components(0),
                                components(1),
                                components(2))
           * QuaternionFromUrdfRPY(components(3),
                                   components(4),
                                   components(5));
  }
  else
  {
    throw std::invalid_argument(
          "VectorXd source vector is not 6 elements in size");
  }
}

Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform)
{
  Eigen::VectorXd components = Eigen::VectorXd::Zero(6);
  const Eigen::Vector3d translation = transform.translation();
  const Eigen::Vector3d rotation
      = EulerAnglesFromRotationMatrix(transform.rotation());
  components << translation, rotation;
  return components;
}

Eigen::Vector3d StdVectorDoubleToEigenVector3d(
    const std::vector<double>& vector)
{
  if (vector.size() == 3)
  {
    return Eigen::Vector3d(vector.at(0), vector.at(1), vector.at(2));
  }
  else
  {
    throw std::invalid_argument(
          "Vector3d source vector is not 3 elements in size");
  }
}

Eigen::VectorXd StdVectorDoubleToEigenVectorXd(
    const std::vector<double>& vector)
{
  Eigen::VectorXd eigen_vector(vector.size());
  for (size_t idx = 0; idx < vector.size(); idx++)
  {
    const double val = vector.at(idx);
    eigen_vector(static_cast<ssize_t>(idx)) = val;
  }
  return eigen_vector;
}

std::vector<double> EigenVector3dToStdVectorDouble(
    const Eigen::Vector3d& point)
{
  return std::vector<double>{point.x(), point.y(), point.z()};
}

std::vector<double> EigenVectorXdToStdVectorDouble(
    const Eigen::VectorXd& eigen_vector)
{
  std::vector<double> vector(static_cast<size_t>(eigen_vector.size()));
  for (size_t idx = 0; idx < vector.size(); idx++)
  {
    const double val = eigen_vector(static_cast<ssize_t>(idx));
    vector.at(idx) = val;
  }
  return vector;
}

// Takes <x, y, z, w> as is the ROS custom!
Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(
    const std::vector<double>& vector)
{
  if (vector.size() == 4)
  {
    return Eigen::Quaterniond(
        vector.at(3), vector.at(0), vector.at(1), vector.at(2));
  }
  else
  {
    throw std::invalid_argument(
          "Quaterniond source vector is not 4 elements in size");
  }
}

// Returns <x, y, z, w> as is the ROS custom!
std::vector<double> EigenQuaterniondToStdVectorDouble(
    const Eigen::Quaterniond& quat)
{
  return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
}
}  // namespace conversions
}  // namespace common_robotics_utilities


#include <fmt/core.h>
#include <yaml-cpp/yaml.h>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <boost/math/quadrature/gauss.hpp>
#include <iostream>
#include <numeric>
#include <optional>
#include <vector>

#include "NumpySaver.hpp"
#include "bsplines.hpp"

using namespace Eigen;
using std::cout, std::endl, std::cerr;

const char* devider = "##########################################";

template <int num_points>
double matrix_B_element(int i, int j, std::vector<double>& knots, Index order) {
  using boost::math::quadrature::gauss;

  if (std::abs((int)i - (int)j) > order) return 0;

  int imax = std::max(i, j);
  int imin = std::min(i, j);

  Index n = knots.size();

  double sum = 0;

  for (int k = std::max(0, imax - 1); k < std::min(imin + order + 1, n - 1);
       k++) {
    auto f = [&](const double& t) {
      auto [values, idx] = bsplines::bsplines(t, knots, order);
      return values[i + order - idx] * values[j + order - idx];
    };
    sum += gauss<double, num_points>::integrate(f, knots[k], knots[k + 1]);
  }

  return sum;
}

template <int num_points>
double matrix_H_element(int i, int j, std::vector<double>& knots, Index order,
                        int l, int z) {
  using boost::math::quadrature::gauss;
  if (std::abs((int)i - (int)j) > order) return 0;

  Index n = knots.size();
  int imax = std::max(i, j);
  int imin = std::min(i, j);

  double sum = 0;

  for (int k = std::max(0, imax - 1); k < std::min(imin + order + 1, n - 1);
       k++) {
    auto f = [&](const double& t) {
      auto [values, idx] = bsplines::bsplines(t, knots, order);
      auto [values_dd, idx_dd] = bsplines::ndx_bsplines(t, knots, order, 2);
      double h_val =
          -values_dd[j - (idx - order)] / 2. +
          (l * (l + 1.) / (t * t * 2.) - z / t) * values[j - (idx - order)];
      return values[i - (idx - order)] * h_val;
      ;
    };

    sum += gauss<double, num_points>::integrate(f, knots[k], knots[k + 1]);
  }
  return sum;
}

template <int num_points>
MatrixXd matrix_B(std::vector<double>& knots, Index order) {
  Index n = knots.size();
  MatrixXd mat(n - 3 + order, n - 3 + order);

  for (int row = 0; row < mat.rows(); row++)
    for (int col = 0; col < mat.cols(); col++)
      mat(row, col) = matrix_B_element<num_points>(
          row - order + 1, col - order + 1, knots, order);

  return mat;
}

template <int num_points>
MatrixXd matrix_H(std::vector<double>& knots, Index order, int l, int z) {
  Index n = knots.size();
  MatrixXd mat(n - 3 + order, n - 3 + order);

  for (int row = 0; row < mat.rows(); row++)
    for (int col = 0; col < mat.cols(); col++)
      mat(row, col) = matrix_H_element<num_points>(
          row - order + 1, col - order + 1, knots, order, l, z);

  return mat;
}

double evaluate_spline(const double& x, const std::vector<double>& knots,
                       const VectorXd& weights, std::size_t order) {
  auto n = knots.size();
  assert(int(n + order) - 3 == weights.size());

  auto [values, index] = bsplines::bsplines(x, knots, order);

  double sum = 0;
  for (std::size_t i = std::max(int(index) - 1, 0);
       i <= std::min(index - 1 + order, n + order - 4); i++)
    sum += values[i + 1 - index] * weights[i];

  return sum;
}

template <int order>
void solve(std::vector<double>& knots, std::string& basefilename,
           std::optional<double> plot_max, int l, int z) {
  constexpr int gauss_points = order * 1.5;
  int n_knots = knots.size();

  cout << "knots = ";
  for (auto k : knots) cout << k << ", ";
  cout << endl;

  auto B = matrix_B<gauss_points>(knots, order);
  auto H = matrix_H<gauss_points>(knots, order, l, z);

  if (knots.size() < 20) {
    cout << "B=\n" << B << "\n\n";
    cout << "H=\n" << H << endl;
  }

  GeneralizedEigenSolver<MatrixXd> ges;
  ges.compute(H, B);
  VectorXcd eigenvalues = ges.eigenvalues();
  VectorXd real_eigen = eigenvalues.real();

  // sort the eigenvalues (and create an index list)
  std::vector<int> indices(real_eigen.size());
  std::iota(indices.begin(), indices.end(), 0);

  std::sort(indices.begin(), indices.end(), [&](int A, int B) -> bool {
    return real_eigen[A] < real_eigen[B];
  });
  std::sort(real_eigen.data(), real_eigen.data() + real_eigen.size());

  if (eigenvalues.imag().cwiseAbs().sum() > 10e-10) {
    cerr << "Error: Complex eigenvalues!" << endl;
  }

  cout << "Eigenvalues: " << real_eigen.transpose() << endl;

  MatrixXd eigenvectors = ges.eigenvectors().real();

  NumpySaver(fmt::format("build/output/{}_eigenvalues.npy", basefilename))
      << real_eigen;
  NumpySaver(fmt::format("build/output/{}_knots.npy", basefilename)) << knots;

  NumpySaver eigvec_saver(
      fmt::format("build/output/{}_eigenvectors.npy", basefilename));
  for (int col = 0; col < eigenvectors.cols(); col++)
    eigvec_saver << eigenvectors.col(indices[col]);

  if (auto xmax = plot_max) {
    ArrayXd x = ArrayXd::LinSpaced(1000, 0, *xmax);
    MatrixXd points(x.size(), eigenvectors.cols());
    for (int row = 0; row < x.size(); row++)
      for (int col = 0; col < points.cols(); col++)
        points(row, col) = evaluate_spline(
            x[row], knots, eigenvectors.col(indices[col]), order);

    NumpySaver plot_saver(
        fmt::format("build/output/{}_plot_points.npy", basefilename));
    plot_saver << x;
    for (int col = 0; col < points.cols(); col++) plot_saver << points.col(col);
  }
}

template <int order>
void solve_exp(int n_knots, double rmin, double rmax, double exp_fak,
               std::string& basefilename, std::optional<double> plot, int l,
               int z) {
  std::vector<double> knots(n_knots);
  for (int i = 0; i < n_knots; i++)
    knots[i] = (rmax - rmin) *
                   (std::exp(double(i) / double(n_knots - 1) * exp_fak) - 1.) /
                   (std::exp(exp_fak) - 1) +
               rmin;

  solve<order>(knots, basefilename, plot, l, z);
}

template <int order>
void solve_lin(int n_knots, double rmin, double rmax, std::string& basefilename,
               std::optional<double> plot, int l, int z) {
  std::vector<double> knots(n_knots);
  for (int i = 0; i < n_knots; i++)
    knots[i] = double(i) / double(n_knots - 1) * (rmax - rmin) + rmin;

  solve<order>(knots, basefilename, plot, l, z);
}

int main() {
  YAML::Node configs = YAML::LoadFile("config.yaml");

  constexpr int order = 3;

  for (std::size_t i = 0; i < configs.size(); i++) {
    YAML::Node config = configs[i];

    std::optional<double> plot;

    if (auto p_node = config["plot_xmax"]) {
      try {
        auto val = p_node.as<double>();
        if (val > 0) plot.emplace(val);
      } catch (const YAML::TypedBadConversion<double>&) {
        auto boolval = p_node.as<bool>();
        if (boolval) plot.emplace(10);
      }
    }

    auto l = config["l"].as<int>();
    auto z = config["Z"].as<int>();
    auto basefilename = config["name"].as<std::string>();

    fmt::print("\n\n{0}\n# Simulation '{1}' {2}/{3}\n{0}\n", devider,
               basefilename, i + 1, configs.size());

    YAML::Node knot_node = config["knots"];
    auto type = knot_node.Type();
    // single value
    if (type == YAML::NodeType::Sequence) {
      auto knots = knot_node.as<std::vector<double>>();
      solve<order>(knots, basefilename, plot, l, z);
    } else if (type == YAML::NodeType::Map) {
      auto rmin = knot_node["rmin"].as<double>();
      auto rmax = knot_node["rmax"].as<double>();
      auto N = knot_node["N"].as<size_t>();
      auto type = knot_node["type"].as<std::string>();
      if (type == "linear") {
        solve_lin<order>(N, rmin, rmax, basefilename, plot, l, z);
      } else if (type == "exponential") {
        auto expfac = knot_node["expfac"].as<double>();
        solve_exp<order>(N, rmin, rmax, expfac, basefilename, plot, l, z);
      } else {
        throw std::runtime_error(
            "'type' of knots must either be 'linear' or 'exponential'.");
      }
    } else {
      throw std::runtime_error(
          "Range node must either be list, scalar or a map defining "
          "'type', 'N', 'rmin' and 'rmax'.");
    }
  }
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__INTERP__SPLINE_HPP_
#define CBR_MATH__INTERP__SPLINE_HPP_

#include <Eigen/Dense>

#include <utility>
#include <vector>
#include <array>

#include "piecewise_poly.hpp"

namespace cbr
{

class Spline
{
protected:
  using row_t = Eigen::Matrix<double, 1, Eigen::Dynamic>;
  using matrix_t = Eigen::MatrixXd;

public:
  /**
   * @brief Create a scalar-valued spline
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::matrix_t
   * @param x breakpoints of shape 1xN
   * @param y values of shape DxN where D \in {1, 2, 3}
   *
   * D=1: y contains values
   * D=2: y contains values, 1st derivatives
   * D=3: y contains values, 1st derivatives, 2nd derivatives
   */
  template<typename T1, typename T2>
  static PiecewisePoly fit(
    T1 && x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() < 2) {
      throw std::invalid_argument("x be of size > 1");
    }

    if (x.size() != y.cols()) {
      throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
    }

    if (!(y.rows() > 0 && y.rows() < 4)) {
      throw std::invalid_argument("y must have between 1 and 3 rows.");
    }

    matrix_t coeffs = generateCoeffs(x, y);

    if (y.rows() == 1 && x.size() == 3) {
      Eigen::Matrix<double, 1, 2> bp{x[0], x[2]};
      return PiecewisePoly(std::move(bp), std::move(coeffs));
    } else {
      return PiecewisePoly(std::forward<T1>(x), std::move(coeffs));
    }
  }

  /**
   * @brief Create a vector-valued spline
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 container_t<PiecewisePoly::matrix_t>
   * @param x breakpoints of shape 1xN
   * @param y values of shape MxDxN where M is number of value dimensions and D \in {1, 2, 3}
   *
   * D=1: y[m] contains values for m:th dimension
   * D=2: y[m] contains values, 1st derivatives for m:th dimension
   * D=3: y[m] contains values, 1st, 2nd derivatives for m:th dimension
   */
  template<typename T1, typename T2>
  static PiecewisePolyND fitND(
    T1 && x,
    const T2 & ys)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() < 2) {
      throw std::invalid_argument("x must be of size > 1");
    }

    if constexpr (is_eigen_dense_v<T2>) {
      if (ys.rows() < 1) {
        throw std::invalid_argument("Number of rows of ys must be > 0.");
      }

      if (x.size() != ys.cols()) {
        throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
      }

      std::vector<matrix_t> coefLists;
      coefLists.reserve(static_cast<std::size_t>(ys.rows()));

      for (Eigen::Index i = 0; i < ys.rows(); i++) {
        coefLists.push_back(generateCoeffs(x, ys.row(i)));
      }

      return PiecewisePolyND(std::forward<T1>(x), std::move(coefLists));
    } else {
      static_assert(
        is_eigen_dense_v<typename T2::value_type>,
        "ys must be a container of Eigen::DenseBase objects");

      if (ys.size() < 1) {
        throw std::invalid_argument("Number of elements of ys must be > 0.");
      }

      const auto ni = ys[0].rows();
      if (!(ni > 0 && ni < 4)) {
        throw std::invalid_argument("Each element of ys must have between 1 and 3 rows.");
      }
      for (const auto & y : ys) {
        if (y.rows() != ni) {
          throw std::invalid_argument(
                  "Each element of ys must have the same number of rows.");
        }
      }

      std::vector<matrix_t> coefLists;
      coefLists.reserve(ys.size());
      for (const auto & y : ys) {
        coefLists.push_back(generateCoeffs(x, y));
      }

      if (ni == 1 && x.size() == 3) {
        Eigen::Matrix<double, 1, 2> bp{x[0], x[2]};
        return PiecewisePolyND(std::move(bp), std::move(coefLists));
      } else {
        return PiecewisePolyND(std::forward<T1>(x), std::move(coefLists));
      }
    }
  }

protected:
  template<typename T1, typename T2>
  static matrix_t generateCoeffs(
    const Eigen::DenseBase<T1> & x,
    const Eigen::DenseBase<T2> & y)
  {
    if (x.size() != y.cols()) {
      throw std::invalid_argument(
              "The number of columns of each element of ys must be equal to the size of x.");
    }

    matrix_t coeffs;

    const Eigen::Index nx = x.size();
    const Eigen::Index nj = nx - 1;
    const Eigen::Index ni = y.rows();

    if (ni == 1) {
      if (nx == 2) {  // If only 2 points, linear interpolation
        coeffs.resize(2, 1);
        coeffs(1, 0) = y(0, 0);
        coeffs(0, 0) = (y(0, 1) - y(0, 0)) / (x[1] - x[0]);

      } else if (nx == 3) {  // If only 3 points, quadratic interpolation
        coeffs.resize(3, 1);

        Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
        Eigen::Vector3d b = Eigen::Vector3d::Zero();

        A(0, 2) = 1.;

        A(1, 0) = powFast<2>(x[1] - x[0]);
        A(1, 1) = (x[1] - x[0]);
        A(1, 2) = 1.;

        A(2, 0) = powFast<2>(x[2] - x[0]);
        A(2, 1) = (x[2] - x[0]);
        A(2, 2) = 1.;

        b[0] = y(0, 0);
        b[1] = y(0, 1);
        b[2] = y(0, 2);

        Eigen::FullPivLU<Eigen::Matrix3d> dec(A);
        coeffs = dec.solve(b);

      } else {  // If more than 3 points, piecewise cubic interpolation
        coeffs.resize(4, nj);
        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(4 * nj, 4 * nj);
        Eigen::VectorXd b = Eigen::VectorXd::Zero(4 * nj);

        // Function continuity
        Eigen::Index i0 = 0;
        for (Eigen::Index i = 0; i < nx - 1; i++) {
          A(2 * i, 4 * i) = powFast<3>(x[i + 1] - x[i]);
          A(2 * i, 4 * i + 1) = powFast<2>(x[i + 1] - x[i]);
          A(2 * i, 4 * i + 2) = (x[i + 1] - x[i]);
          A(2 * i, 4 * i + 3) = 1.;
          A(2 * i + 1, 4 * i + 3) = 1.;

          b[2 * i] = y(0, i + 1);
          b[2 * i + 1] = y(0, i);
        }

        // First derivative continuity
        i0 += 2 * (nx - 1);
        for (Eigen::Index i = 0; i < nx - 2; i++) {
          A(i0 + i, 4 * i) = 3. * powFast<2>(x[i + 1] - x[i]);
          A(i0 + i, 4 * i + 1) = 2. * (x[i + 1] - x[i]);
          A(i0 + i, 4 * i + 2) = 1.;
          A(i0 + i, 4 * i + 6) = -1.;
        }

        // Second derivatives continuity
        i0 += (nx - 2);
        for (Eigen::Index i = 0; i < nx - 2; i++) {
          A(i0 + i, 4 * i) = 6. * (x[i + 1] - x[i]);
          A(i0 + i, 4 * i + 1) = 2.;
          A(i0 + i, 4 * i + 5) = -2.;
        }

        // Not-a-knot conditions
        i0 += (nx - 2);
        A(i0, 0) = 6.;
        A(i0, 4) = -6.;
        A(i0 + 1, 4 * (nx - 3)) = 6.;
        A(i0 + 1, 4 * (nx - 3) + 4) = -6.;

        // Solve linear system
        Eigen::FullPivLU<Eigen::MatrixXd> dec(A);
        Eigen::Map<Eigen::VectorXd> coeffsMap(coeffs.data(), 4 * nj);
        coeffsMap = dec.solve(b);
      }

    } else if (ni == 2) {
      coeffs.resize(4, nj);
      for (Eigen::Index i = 0; i < nj; i++) {
        Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
        Eigen::Vector4d b = Eigen::Vector4d::Zero();

        // Function continuity
        A(0, 0) = powFast<3>((x[i + 1] - x[i]));
        A(0, 1) = powFast<2>((x[i + 1] - x[i]));
        A(0, 2) = (x[i + 1] - x[i]);
        A(0, 3) = 1.;
        A(1, 3) = 1.;
        b[0] = y(0, i + 1);
        b[1] = y(0, i);

        // First derivatives continuity
        A(2, 0) = 3. * powFast<2>((x[i + 1] - x[i]));
        A(2, 1) = 2. * (x[i + 1] - x[i]);
        A(2, 2) = 1.;
        A(3, 2) = 1.;
        b[2] = y(1, i + 1);
        b[3] = y(1, i);

        // Solve linear system
        Eigen::FullPivLU<Eigen::MatrixXd> dec(A);
        coeffs.col(i) = dec.solve(b);
      }

    } else if (ni == 3) {
      coeffs.resize(6, nj);
      for (Eigen::Index i = 0; i < nj; i++) {
        Eigen::Matrix<double, 6, 6> A = Eigen::Matrix<double, 6, 6>::Zero();
        Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();

        // Function continuity
        A(0, 0) = powFast<5>((x[i + 1] - x[i]));
        A(0, 1) = powFast<4>((x[i + 1] - x[i]));
        A(0, 2) = powFast<3>((x[i + 1] - x[i]));
        A(0, 3) = powFast<2>((x[i + 1] - x[i]));
        A(0, 4) = (x[i + 1] - x[i]);
        A(0, 5) = 1.;
        A(1, 5) = 1.;
        b[0] = y(0, i + 1);
        b[1] = y(0, i);

        // First derivatives continuity
        A(2, 0) = 5. * powFast<4>((x[i + 1] - x[i]));
        A(2, 1) = 4. * powFast<3>((x[i + 1] - x[i]));
        A(2, 2) = 3. * powFast<2>((x[i + 1] - x[i]));
        A(2, 3) = 2. * (x[i + 1] - x[i]);
        A(2, 4) = 1.;
        A(3, 4) = 1.;
        b[2] = y(1, i + 1);
        b[3] = y(1, i);

        // Second derivatives continuity
        A(4, 0) = 20. * powFast<3>((x[i + 1] - x[i]));
        A(4, 1) = 12. * powFast<2>((x[i + 1] - x[i]));
        A(4, 2) = 6. * (x[i + 1] - x[i]);
        A(4, 3) = 2.;
        A(5, 3) = 2.;
        b[4] = y(2, i + 1);
        b[5] = y(2, i);

        // Solve linear system
        Eigen::FullPivLU<Eigen::Matrix<double, 6, 6>> dec(A);
        coeffs.col(i) = dec.solve(b);
      }

    } else {
      throw std::invalid_argument("y as an unsupported number of rows. Should never happen!");
    }
    return coeffs;
  }
};

}  // namespace cbr

#endif  // CBR_MATH__INTERP__SPLINE_HPP_


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout; using std::endl;

// Setting (approximate) precision 25 bits in a single function call using make_policy.

//[policy_ref_snip11

#include <boost/math/distributions/normal.hpp>
using boost::math::normal_distribution;

using namespace boost::math::policies;

const int bits = 25; // approximate precision.

double q = quantile(
      normal_distribution<double, policy<digits2<bits> > >(),
      0.05); // 5% quantile.

//] //[/policy_ref_snip11]

int main()
{
  std::streamsize p = 2 + (bits * 30103UL) / 100000UL;
  // Approximate number of significant decimal digits for 25 bits.
  cout.precision(p);
  cout << bits << " binary bits is approoximately equivalent to " << p << " decimal digits " << endl;
  cout << "quantile(normal_distribution<double, policy<digits2<25> > >(), 0.05  = "
     << q << endl; // -1.64485
}

/*
Output:
  25 binary bits is approoximately equivalent to 9 decimal digits
  quantile(normal_distribution<double, policy<digits2<25> > >(), 0.05  = -1.64485363
  */


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

// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ARITHMETIC_CROSS_PRODUCT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ARITHMETIC_CROSS_PRODUCT_HPP

#include <cstddef>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename P1, typename P2, std::size_t Dimension>
struct cross_product
{
    // We define cross product only for 2d (see Wolfram) and 3d.
    // In Math, it is also well-defined for 7-dimension.
    // Generalisation of cross product to n-dimension is defined as
    // wedge product but it is not direct analogue to binary cross product.
};

template <typename P1, typename P2>
struct cross_product<P1, P2, 2>
{
    typedef P1 return_type;

    static inline return_type apply(P1 const& p1, P2 const& p2)
    {
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();

        // For 2-dimensions, analog of the cross product U(x,y) and V(x,y) is
        // Ux * Vy - Uy * Vx
        // which is returned as 0-component (or X) of 2d vector, 1-component is undefined.
        return_type v;
        set<0>(v, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
        return v;
    }
};

template <typename P1, typename P2>
struct cross_product<P1, P2, 3>
{
    typedef P1 return_type;

    static inline return_type apply(P1 const& p1, P2 const& p2)
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();

        return_type v;
        set<0>(v, get<1>(p1) * get<2>(p2) - get<2>(p1) * get<1>(p2));
        set<1>(v, get<2>(p1) * get<0>(p2) - get<0>(p1) * get<2>(p2));
        set<2>(v, get<0>(p1) * get<1>(p2) - get<1>(p1) * get<0>(p2));
        return v;
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

// TODO: This is a simple draft. If relevant, it can be extended to:
// - accept vectors of different coordinate_type, but common coordinate_system
// - if vectors are of mixed 2d and 3d, lower dimension is used
// - define result_type that will generate type of vector based on:
// -- select_coordinate_type
// -- selection of lower dimension

/*!
\brief Computes the cross product of two vectors.
\details Both vectors shall be of the same type.
         This type also determines type of result vector.
\ingroup arithmetic
\param p1 first vector
\param p2 second vector
\return the cross product vector
 */
template <typename P1, typename P2>
inline P1 cross_product(P1 const& p1, P2 const& p2)
{
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<P1>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<P2>) );

    return detail::cross_product
        <
            P1, P2,
            dimension<P1>::type::value
        >::apply(p1, p2);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ARITHMETIC_CROSS_PRODUCT_HPP


#ifndef UTILS_H
#define UTILS_H

#define _USE_MATH_DEFINES
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <algorithm>

// Clamp a to be within lim1 and lim2
double clamp(double a, double lim1, double lim2);
// Scales input to be between (0 to 1) based on input limits
double scaleFactor(double f, double tl, double tu);
// Converts a rotation matrix to Euler angles (RzRyRx) convention
Eigen::Vector3d Rotation2Euler(const Eigen::Matrix3d& R);
// Computes an iterative inverse kinematics solution
// Eigen::VectorXd IterativeInverseKinematics(std::function<Eigen::VectorXd(const Eigen::VectorXd&)> FK_func, std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> J_FK_func, const Eigen::VectorXd& x_target, const Eigen::VectorXd& theta_init);
// Computes an iterative inverse kinematics solution with holonomic constraints
// Eigen::VectorXd IterativeConstrainedInverseKinematics(std::function<Eigen::VectorXd(const Eigen::VectorXd&)> y_func, std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> J_y_func, std::function<Eigen::VectorXd(const Eigen::VectorXd&)> hol_func, std::function<Eigen::MatrixXd(const Eigen::VectorXd&)> J_hol_func, const Eigen::VectorXd& y_target, const Eigen::VectorXd& q_init);
// Convert from euler angles to rotation matrix using the ZYX convention
Eigen::Matrix3d Euler2Rotation(const Eigen::Vector3d& euler);
// Convert from angular velocity to euler rates using the ZYX convention
Eigen::Vector3d AngularVelocity2EulerRates(const Eigen::Vector3d& euler, const Eigen::Vector3d& w);
// Convert from to euler rates to angular velocity using the ZYX convention
Eigen::Vector3d EulerRates2AngularVelocity(const Eigen::Vector3d& euler, const Eigen::Vector3d& euler_rates);
// Computes skew-symmetric matrix from Vector3d
Eigen::Matrix3d skew(const Eigen::Vector3d& v);

#endif // UTILS_H


/* boost random/beta_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_BETA_DISTRIBUTION_HPP
#define BOOST_RANDOM_BETA_DISTRIBUTION_HPP

#include <cassert>
#include <istream>
#include <iosfwd>
#include <boost/random/detail/operators.hpp>
#include <boost/random/gamma_distribution.hpp>

namespace boost {
namespace random {

/**
 * The beta distribution is a real-valued distribution which produces
 * values in the range [0, 1].  It has two parameters, alpha and beta.
 *
 * It has \f$\displaystyle p(x) = \frac{x^{\alpha-1}(1-x)^{\beta-1}}{B(\alpha, \beta)}\f$.
 */
template<class RealType = double>
class beta_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef beta_distribution distribution_type;

        /**
         * Constructs a @c param_type from the "alpha" and "beta" parameters
         * of the distribution.
         *
         * Requires: alpha > 0, beta > 0
         */
        explicit param_type(RealType alpha_arg = RealType(1.0),
                            RealType beta_arg = RealType(1.0))
          : _alpha(alpha_arg), _beta(beta_arg)
        {
            assert(alpha_arg > 0);
            assert(beta_arg > 0);
        }

        /** Returns the "alpha" parameter of the distribtuion. */
        RealType alpha() const { return _alpha; }
        /** Returns the "beta" parameter of the distribution. */
        RealType beta() const { return _beta; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._alpha << ' ' << parm._beta; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._alpha >> std::ws >> parm._beta; return is; }

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

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

    private:
        RealType _alpha;
        RealType _beta;
    };

    /**
     * Constructs an @c beta_distribution from its "alpha" and "beta" parameters.
     *
     * Requires: alpha > 0, beta > 0
     */
    explicit beta_distribution(RealType alpha_arg = RealType(1.0),
                               RealType beta_arg = RealType(1.0))
      : _alpha(alpha_arg), _beta(beta_arg)
    {
        assert(alpha_arg > 0);
        assert(beta_arg > 0);
    }
    /** Constructs an @c beta_distribution from its parameters. */
    explicit beta_distribution(const param_type& parm)
      : _alpha(parm.alpha()), _beta(parm.beta())
    {}

    /**
     * Returns a random variate distributed according to the
     * beta distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng) const
    {
        RealType a = gamma_distribution<RealType>(_alpha, RealType(1.0))(urng);
        RealType b = gamma_distribution<RealType>(_beta, RealType(1.0))(urng);
        return a / (a + b);
    }

    /**
     * Returns a random variate distributed accordint to the beta
     * distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return beta_distribution(parm)(urng);
    }

    /** Returns the "alpha" parameter of the distribution. */
    RealType alpha() const { return _alpha; }
    /** Returns the "beta" parameter of the distribution. */
    RealType beta() const { return _beta; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return RealType(0.0); }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return RealType(1.0); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_alpha, _beta); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _alpha = parm.alpha();
        _beta = parm.beta();
    }

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

    /** Writes an @c beta_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, beta_distribution, wd)
    {
        os << wd.param();
        return os;
    }

    /** Reads an @c beta_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, beta_distribution, wd)
    {
        param_type parm;
        if(is >> parm) {
            wd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two instances of @c beta_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(beta_distribution, lhs, rhs)
    { return lhs._alpha == rhs._alpha && lhs._beta == rhs._beta; }

    /**
     * Returns true if the two instances of @c beta_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(beta_distribution)

private:
    RealType _alpha;
    RealType _beta;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_BETA_DISTRIBUTION_HPP


#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <cmath>
#include <cassert>
#include <chrono>
#include "writer.hpp"

using namespace std::chrono;
using Real= double;
using SparseMatrix= Eigen::SparseMatrix<Real>;
using Triplet= Eigen::Triplet<Real>;
using Vector= Eigen::VectorXd;

template <typename T>
class Rho
{
    public:
        Rho(const T x_0, const T y_0, const T dx)
         : x_0(x_0), y_0(y_0), dx(dx) {}
        T operator() (const T x, const T y)
        {
           if (!set && abs(x - x_0) < dx / 2 && abs(y - y_0) < dx / 2)
           {
               set= true;
               return (T)1.0 / dx / dx;
           }
           else
           {
               return (T)0.0;
           }
        }
    private:
        const T x_0, y_0, dx;
        bool set{false};
};

void CreatePoissonMatrix(SparseMatrix& A, const int N)
{
    A.resize(N * N, N * N); // resize for unrolling RHS vector
    std::vector<Triplet> triplets; // to build sparse matrix A from
    triplets.reserve(N * N * 5); // each row of A in general has 5 entries - will be somewhat too large
    for (int i= 0; i < N * N; i++)
    {
        triplets.push_back(Triplet(i, i, 4)); // main diagonal
        if (i > 0 && i % N != 0)
            triplets.push_back(Triplet(i, i - 1, -1)); // left off-diagonal: i-1
        if (i < N * N - 1 && (i + 1) % N != 0 || i == 0)
            triplets.push_back(Triplet(i, i + 1, -1)); // right off-diagonal: i+1
        if (i < N * N - N)
            triplets.push_back(Triplet(i, i + N, -1)); // j+1
        if (i > N - 1)
            triplets.push_back(Triplet(i, i - N, -1)); // j-1
    }
    A.setFromTriplets(triplets.begin(), triplets.end());
}

template <class F, typename T>
void CreateRHSVector(Vector& RHS, const int N, const T dx, F& rho)
{
    RHS.resize(N * N); // unroll grid into vector

    auto x= [&dx](T i) {return (i + 1) * dx;};
    auto y= [&dx](T j) {return (j + 1) * dx;};

    const T dx2{dx * dx};

    for (int j= 0; j < N; j++) // row major storage
        for (int i= 0; i < N; i++)
            RHS[j * N + i]= dx2 * rho(x(i), y(j));
}

template <typename T>
Vector SolveJacobi(const SparseMatrix& A, const Vector& RHS, const int N,
                   const T treshold, const int iterations)
{
    Vector x= Vector::Zero((N + 2) * (N + 2));
    Vector x_0= Vector::Ones(N * N);
    Vector x_1;

    int i{0};
    T l2_norm{1.0};

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    const SparseMatrix D(A.diagonal().asDiagonal());
    const SparseMatrix D_inv(D.diagonal().asDiagonal().inverse());
    const SparseMatrix R= A - D;

    const Vector D_inv_x_RHS= D_inv * RHS; // to speed up calculations during loop
    const SparseMatrix D_inv_x_R= D_inv * R;

    while (l2_norm > treshold && i++ < iterations)
    {
        x_1= D_inv_x_RHS - D_inv_x_R * x_0; // x_1= D_inv * (RHS - R * x_0)
        l2_norm= (x_1 - x_0).norm();
        x_0= x_1;
    }
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Jacobi relaxation solver: Success"
              << " after " << i << " iterations in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;

    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_1[(j - 1) * N + (i - 1)];
    return x;
}

template <typename T>
Vector SolveGaussSeidel(const SparseMatrix& A, const Vector& RHS, const int N,
                        const T treshold, const int iterations)
{
    Vector x= Vector::Zero((N + 2) * (N + 2));
    Vector x_0= Vector::Ones(N * N);
    Vector x_1;

    int i{0};
    T l2_norm{1.0};

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    const SparseMatrix L= A.triangularView<Eigen::Lower>();
    const SparseMatrix U= A.triangularView<Eigen::StrictlyUpper>();

    Eigen::SparseLU<SparseMatrix> solver; // to get L^{-1}
    solver.compute(L);
    SparseMatrix I(N * N, N * N); I.setIdentity();
    const SparseMatrix L_inv= solver.solve(I);

    const Vector L_inv_x_RHS= L_inv * RHS; // to speed up calculations during loop
    const SparseMatrix L_inv_x_U= L_inv * U;

    while (l2_norm > treshold && i++ < iterations)
    {
        x_1= L_inv_x_RHS - L_inv_x_U * x_0; // x_1= L_inv * (RHS - U * x_0)
        l2_norm= (x_1 - x_0).norm();
        x_0= x_1;
    }
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Gauss-Seidel solver: Success"
              << " after " << i << " iterations in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;

    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_1[(j - 1) * N + (i - 1)];
    return x;
}

template <typename T>
Vector SolveConjugateGradient(const SparseMatrix& A, const Vector& RHS, const int N,
                              const T treshold, const int iterations, const bool use_jacobi_precond=false)
{
    Vector x= Vector::Zero((N + 2) * (N + 2)); // to hold final solution w/ boundary conditions
    Vector x_= Vector::Zero(N * N); // for iterative calculations
    SparseMatrix M_inv= SparseMatrix(N * N, N * N); M_inv.setIdentity(); // no preconditioner
    if (use_jacobi_precond)
        M_inv= A.diagonal().asDiagonal().inverse(); // Jacobi preconditioner
    Vector r_0, r_1; // residuals r
    Vector p, Ap; // "direction vectors" p
    Vector z_0, z_1; // preconditioned residuals z
    T alpha, beta; // Richardson iteration scaling parameters

    int i{0};

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    r_0= RHS - A * x_;
    z_0= M_inv * r_0;
    p= z_0;

    for ( ; i++ < iterations; )
    {
        Ap= A * p;
        alpha= r_0.dot(z_0) / p.dot(Ap);
        x_= x_ + alpha * p;
        r_1= r_0 - alpha * Ap;
        if (r_1.norm() < treshold) break; // convergence test
        z_1= M_inv * r_1;
        beta= z_1.dot(r_1) / z_0.dot(r_0);
        p= z_1 + beta * p;
        r_0= r_1;
        z_0= z_1;
    }

    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << ((use_jacobi_precond == false) ? "Conjugate gradient solver: Success"
                                                : "Conjugate gradient solver w/ Jacobi preconditioner: Success" )
              << " after " << i << " iterations in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;

    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_[(j - 1) * N + (i - 1)];
    return x;
}

Vector SolveEigenCG(const SparseMatrix& A, const Vector& RHS, const int N)
{
    Vector x= Vector::Zero((N + 2) * (N + 2)); // to fill with solution

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    Eigen::ConjugateGradient<SparseMatrix, Eigen::Lower|Eigen::Upper> solver;
    solver.compute(A);
    assert (solver.info() == Eigen::Success);
    Vector x_inner= solver.solve(RHS); // solve inner grid
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Eigen CG solver: Success in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;
    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_inner[(j - 1) * N + (i - 1)];
    return x;
}

Vector SolveCholesky(const SparseMatrix& A, const Vector& RHS, const int N)
{
    Vector x= Vector::Zero((N + 2) * (N + 2)); // to fill with solution

    high_resolution_clock::time_point t_0= high_resolution_clock::now();
    Eigen::SparseLU<SparseMatrix> solver;
    solver.compute(A);
    assert (solver.info() == Eigen::Success);
    Vector x_inner= solver.solve(RHS); // solve inner grid
    high_resolution_clock::time_point t_1= high_resolution_clock::now();
    std::cout << "Eigen Cholesky solver: Success in "
              << duration_cast<milliseconds>(t_1 - t_0).count() <<" ms" << std::endl;
    for (int j= 1; j <= N; j++)
        for (int i= 1; i <= N; i++)
                x[j * (N + 2) + i]= x_inner[(j - 1) * N + (i - 1)];
    return x;
}

int main(int argc, char* argv[])
{
     if (argc < 4 || argc > 4 || argv[1] == "-h") // check cl args and give some help
    {
        std::cerr << "Usage: " << argv[0] << "\n\t"
                  << " N(int): grid parameter: number of points per axis" << "\n\t"
                  << " x_0(Real): x coord of point charge in 0..1" << "\n\t"
                  << " y_0(Real): y coord of point charge in 0..1" << "\n\t"
                  << std::endl << std::endl;
        return 1;
    }

    const int N{atoi(argv[1])};
    const Real dx{(Real)1.0 / (Real)(N + 1)};
    const Real x_0{atof(argv[2])}, y_0{atof(argv[3])};

    const Real treshold{0.0001};
    const int iterations{10000};

    SparseMatrix A;
    Vector RHS;
    Vector RHS2;
    Rho rho{x_0, y_0, dx};
    Rho rho2{1.0 - x_0, 1.0 - y_0, dx};
    CreatePoissonMatrix(A, N);
    CreateRHSVector(RHS, N, dx, rho);
    CreateRHSVector(RHS2, N, dx, rho2);
    RHS = (RHS + RHS2) / 2;
    Vector x_cholesky= SolveCholesky(A, RHS, N);
    Vector x_eigen_cg= SolveEigenCG(A, RHS, N);
    // Vector x_jacobi= SolveJacobi(A, RHS, N, treshold, iterations);
    // Vector x_gauss_seidel= SolveGaussSeidel(A, RHS, N, treshold, iterations);
    Vector x_conjugate_gradient= SolveConjugateGradient(A, RHS, N, treshold, iterations);
    Vector x_conjugate_gradient_precon= SolveConjugateGradient(A, RHS, N, treshold, iterations, true);
    // std::cout << "L2 difference Cholesky - Jacobi: " << (x_cholesky - x_jacobi).norm() << std::endl
            //   << "L2 difference Cholseky - Gauss-Seidel: " << (x_cholesky - x_jacobi).norm() << std::endl;
    std::cout << "L2 difference Cholesky - Conjugate Gradient: " << (x_cholesky - x_conjugate_gradient).norm() << std::endl;
    std::cout << "L2 difference Eigen CG - Conjugate Gradient: " << (x_eigen_cg - x_conjugate_gradient).norm() << std::endl;
    std::cout << "L2 difference Eigen CG - Conjugate Gradient Precon: " << (x_eigen_cg - x_conjugate_gradient_precon).norm() << std::endl;
    writeToFile("x_cholesky.txt", x_cholesky);
    writeToFile("x_eigen_cg.txt", x_eigen_cg);
    // writeToFile("x_jacobi.txt", x_jacobi);
    // writeToFile("x_gauss_seidel.txt", x_gauss_seidel);
    writeToFile("x_conjugate_gradient.txt", x_conjugate_gradient);
    writeToFile("x_conjugate_gradient_precon.txt", x_conjugate_gradient_precon);
    // writeToFile("x_diff_chol_jaco.txt", (x_cholesky - x_jacobi));
    // writeToFile("x_diff_chol_gaus.txt", (x_cholesky - x_gauss_seidel));
    writeToFile("x_diff_chol_conj.txt", (x_cholesky - x_conjugate_gradient));
    return 0;
}


#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>
#include <vector>

using namespace boost::numeric::odeint;

/* The type of container used to hold the state vector */
typedef boost::array<double, 3> state_type;

void lorenz(const state_type &x, state_type &dxdt, const double t)
{
    double sigma = 10.0;
    double R = 28.0;
    double b = 8.0 / 3.0;

    dxdt[0] = sigma * (x[1] - x[0]);
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

// An example of observer to record steps in the integration
struct boost_array_observer
{
    std::vector<state_type> &m_states;
    std::vector<double> &m_times;

    boost_array_observer(std::vector<state_type> &states, std::vector<double> &times)
        : m_states(states), m_times(times) {}

    void operator()(const state_type &x, double t)
    {
        m_states.push_back(x);
        m_times.push_back(t);
    }
};

int main()
{
    // this time, let the num of steps be known
    const auto n_steps = 1000;

    // System state initialization with initial condition
    state_type x = {0.0, 1.0, 0.1};

    // Containers to store solution
    std::vector<state_type> x_sol;
    std::vector<double> times;

    // Set stepper
    runge_kutta4<state_type> rk4;

    // Integrate over time
    auto t_init = 0.0;
    auto t_final = 10.0;
    auto dt = 0.001;
    auto steps = integrate_const(
        rk4,
        lorenz,
        x,
        t_init,
        t_final,
        dt,
        boost_array_observer(x_sol, times));

    // Displaying result on terminal
    for (size_t i = 0; i <= steps; i++)
    {
        std::cout << times[i] << '\t' << x_sol[i][0] << '\t' << x_sol[i][1] << '\t' << x_sol[i][2] << '\n';
    }

    return 0;
}


/*
 * neonGX - Transformations.hpp
 *
 * Copyright (c) 2016, cynecx
 *
 * This file is part of neonGX and is distributed under a modified
 * BSD 3-Clause License. See LICENSE.txt for details.
 *
 */

#ifndef NEONGX_TRANSFORMATIONS_H
#define NEONGX_TRANSFORMATIONS_H

#include <cmath>
#include <neonGX/Core/Math/Primitives.hpp>
#include <neonGX/Core/Math/Matrix.hpp>
#include <boost/math/constants/constants.hpp>

//#define USE_CONSTANT_CACHING

namespace neonGX {

  enum class ConstantCacheType {
    sin,
    cos,
    tan
  };

  float ConstantCache(ConstantCacheType type, float val);

  template <typename T>
  static constexpr T PI = boost::math::constants::pi<T>();

  inline constexpr float DegreeToRadian(float degree) {
    return degree * PI<float> / 180.0f;
  }

  inline constexpr float RadianToDegree(float radian) {
    return radian * 180.0f / PI<float>;
  }

  inline constexpr Matrix3 GetTranslationMatrix(float tx, float ty) {
    return {
        1.0f, 0.0f, tx,
        0.0f, 1.0f, ty,
        0.0f, 0.0f, 1.0f
    };
  }

  inline constexpr Matrix3 GetTranslationMatrix(const FPoint& point) {
    return GetTranslationMatrix(point.x, point.y);
  }

  inline constexpr Matrix3 GetScalingMatrix(float sx, float sy) {
    return {
        sx, 0.0f, 0.0f,
        0.0f, sy, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline constexpr Matrix3 GetScalingMatrix(const FPoint& point) {
    return GetScalingMatrix(point.x, point.y);
  }

  inline Matrix3 GetRotationMatrix(float q) {
#ifdef USE_CONSTANT_CACHING
    float qsin = ConstantCache(ConstantCacheType::sin, q);
    float qcos = ConstantCache(ConstantCacheType::cos, q);
#else
    float qsin = sin(q);
    float qcos = cos(q);
#endif

    return {
        qcos, -qsin, 0.0f,
        qsin, qcos, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline Matrix3 GetSkewingMatrix(float x, float y) {
#ifdef USE_CONSTANT_CACHING
    float xtan = ConstantCache(ConstantCacheType::tan, x);
    float ytan = ConstantCache(ConstantCacheType::tan, y);
#else
    float xtan = tan(x);
    float ytan = tan(y);
#endif

    return {
        1.0f, xtan, 0.0f,
        ytan, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline Matrix3 GetSkewingMatrix(const FPoint& point) {
    return GetSkewingMatrix(point.x, point.y);
  }

  inline constexpr Matrix3 GetIdentityTransform() {
    return {
        1.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f
    };
  }

  inline Matrix3 GetToClipspaceTransform(const FSize& destFrame,
                                         const FPoint& sourcePoint,
                                         bool swapHeightSign) {
    // (Position / Resolution) * 2 - 1
    auto toClipspace = Matrix3{
        2.0f / destFrame.width, 0.0f, -1.0f,
        0.0f, (swapHeightSign ? -2.0f : 2.0f) / destFrame.height,
          (swapHeightSign ? 1.0f : -1.0f),
        0.0f, 0.0f, 1.0f
    };

    return toClipspace * GetTranslationMatrix(-sourcePoint.x, -sourcePoint.y);
  }

} // end namespace neonGX

#endif // !NEONGX_TRANSFORMATIONS_H


#ifndef RADIUMENGINE_VECTOR_HPP
#define RADIUMENGINE_VECTOR_HPP

/// This file contains typedefs and basic vector classes and functions

#include <Core/RaCore.hpp>

#include <functional>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <unsupported/Eigen/AlignedVector3>

#include <Core/Math/Math.hpp>

// General config
// Use this to force vec3 to be aligned for vectorization (FIXME not working yet)
// #define CORE_USE_ALIGNED_VEC3

namespace Ra
{
    namespace Core
    {
        //
        // Common vector types
        //
        typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorN;
        typedef Eigen::VectorXf                            VectorNf;
        typedef Eigen::VectorXd                            VectorNd;

        typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
        typedef Eigen::Vector4f             Vector4f;
        typedef Eigen::Vector4d             Vector4d;

#ifndef CORE_USE_ALIGNED_VEC3
        typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
        typedef Eigen::Vector3f             Vector3f;
        typedef Eigen::Vector3d             Vector3d;
#else
        typedef Eigen::AlignedVector3<Scalar> Vector3;
        typedef Eigen::AlignedVector3<float> Vector3f;
        typedef Eigen::AlignedVector3<double> Vector3d;
#endif

        typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
        typedef Eigen::Vector2f             Vector2f;
        typedef Eigen::Vector2d             Vector2d;

        typedef Eigen::VectorXi VectorNi;
        typedef Eigen::Vector2i Vector2i;
        typedef Eigen::Vector3i Vector3i;
        typedef Eigen::Vector4i Vector4i;

        typedef Eigen::Matrix<uint, Eigen::Dynamic, 1> VectorNui;
        typedef Eigen::Matrix<uint, 2, 1>              Vector2ui;
        typedef Eigen::Matrix<uint, 3, 1>              Vector3ui;
        typedef Eigen::Matrix<uint, 4, 1>              Vector4ui;

        //
        // Common matrix types
        //

        typedef Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixN;
        typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
        typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
        typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;

        typedef Eigen::MatrixXf MatrixNf;
        typedef Eigen::Matrix4f Matrix4f;
        typedef Eigen::Matrix3f Matrix3f;
        typedef Eigen::Matrix2f Matrix2f;

        typedef Eigen::MatrixXd MatrixNd;
        typedef Eigen::Matrix4d Matrix4d;
        typedef Eigen::Matrix3d Matrix3d;
        typedef Eigen::Matrix2d Matrix2d;

        typedef Eigen::Matrix< uint, Eigen::Dynamic, Eigen::Dynamic > MatrixNui;

        //typedef Eigen::DiagonalMatrix< Scalar, Eigen::Dynamic > Diagonal;
        typedef Eigen::SparseMatrix< Scalar > Diagonal; // Not optimized for Diagonal matrices, but the operations between Sparse and Diagonal are not defined
        typedef Eigen::SparseMatrix< Scalar > Sparse;

        //
        // Transforms and rotations
        //

        typedef Eigen::Quaternion<Scalar> Quaternion;
        typedef Eigen::Quaternionf        Quaternionf;
        typedef Eigen::Quaterniond        Quaterniond;

        typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Transform;
        typedef Eigen::Affine3f                            Transformf;
        typedef Eigen::Affine3d                            Transformd;

        typedef Eigen::AlignedBox<Scalar, 3> Aabb;
        typedef Eigen::AlignedBox3f          Aabbf;
        typedef Eigen::AlignedBox3d          Aabbd;

        typedef Eigen::AngleAxis<Scalar> AngleAxis;
        typedef Eigen::AngleAxisf        AngleAxisf;
        typedef Eigen::AngleAxisd        AngleAxisd;

        typedef Eigen::Translation<Scalar, 3> Translation;
        typedef Eigen::Translation3f         Translationf;
        typedef Eigen::Translation3d         Translationd;

        inline void print( const MatrixN& matrix );

        //
        // Geometry types
        //

        typedef Eigen::ParametrizedLine< Scalar, 2 > Line2;
        typedef Eigen::ParametrizedLine< Scalar, 3 > Line3;
        typedef Eigen::Hyperplane< Scalar, 3 >       Plane3;

        // Todo : storage transform using quaternions ?

        //
        // Misc types
        //
        typedef Vector4  Color;
        typedef Vector4f Colorf;
        typedef Vector4d Colord;

        //
        // Vector Functions
        //
        namespace Vector
        {

            /// Component-wise floor() function on a floating-point vector.
            template<typename Vector>
            inline Vector floor( const Vector& v );

            /// Component-wise ceil() function on a floating-point vector.
            template<typename Vector>
            inline Vector ceil( const Vector& v );

            /// Component-wise trunc() function on a floating-point vector
            template<typename Vector>
            inline Vector trunc( const Vector& v );

            /// Component-wise clamp() function on a floating-point vector.
            template<typename Vector>
            inline Vector clamp( const Vector& v, const Vector& min, const Vector& max );

            /// Component-wise clamp() function on a floating-point vector.
            template<typename Vector>
            inline Vector clamp( const Vector& v, const Scalar& min, const Scalar& max );

            /// Vector range check, works for any numeric vector.
            template<typename Vector_>
            inline bool checkRange( const Vector_& v, const Scalar& min, const Scalar& max );

            /// Call std::isnormal on vector entries.
            template< typename Vector_ >
            inline bool checkInvalidNumbers(Eigen::Ref<const Vector_> v,
                                             const bool FAIL_ON_ASSERT = false);

            /// Get two vectors orthogonal to a given vector.
            inline void getOrthogonalVectors( const Vector3& fx, Eigen::Ref<Vector3> fy, Eigen::Ref<Vector3> fz );

            /// Get the angle between two vectors. Works for types where the cross product is
            /// defined (i.e. 2D and 3D vectors).
            template<typename Vector_>
            inline Scalar angle( const Vector_& v1, const Vector_& v2 );

            /// Get the spherical linear interpolation between two unit non-colinear vectors.
            /// works for types where the cross-product is defined (i.e. 2D and 3D vectors).
            template<typename Vector_>
            inline Vector_ slerp( const Vector_& v1, const Vector_& v2, Scalar t );

            /// @return the projection of point on the plane define by plane and planeNormal
            inline Vector3 projectOnPlane(const Vector3& planePos, const Vector3& planeNormal, const Vector3& point);

            /// Get the cotangent of the angle between two vectors. Works for vector types where
            /// dot and cross product is defined (2D or 3D vectors).
            template <typename Vector_>
            inline Scalar cotan( const Vector_& v1, const Vector_& v2);

            /// Get the cosine of the angle between two vectors.
            template <typename Vector_>
            inline Scalar cos( const Vector_& v1, const Vector_& v2);

            /// Normalize a vector and returns its norm before normalization.
            /// If the vector's norm is 0, the vector's components will be overwritten by NaNs
            template< typename Vector_ >
            inline Scalar getNormAndNormalize(Vector_& v);

            /// Normalize a vector and returns its norm before normalization.
            /// If the vector's norm is 0, the vector remains null
            template< typename Vector_ >
            inline Scalar getNormAndNormalizeSafe(Vector_& v);

        }

        namespace MatrixUtils
        {
            inline Matrix4 lookAt(const Vector3& position, const Vector3& target, const Vector3& up);
            inline Matrix4 perspective(Scalar fovy, Scalar aspect, Scalar near, Scalar zfar);
            inline Matrix4 orthographic(Scalar left, Scalar right, Scalar bottom, Scalar top, Scalar near, Scalar zfar);

            /// Call std::isnormal on matrix entry.
            /// Dense version
            template< typename Matrix_ >
            inline bool checkInvalidNumbers(Eigen::Ref<const Matrix_> matrix,
                                             const bool FAIL_ON_ASSERT = false);

        }

        //
        // Quaternion functions
        //
        namespace QuaternionUtils
        {
            // Define functions for multiplying a quaternion by a scalar
            // and adding two quaternions. While Quaternion is supposed to
            // represent a unit quaternion (thus a valid rotation), these functions
            // are useful for linear interpolation of quaternions.

            /// Returns the quaternion q multipled by a scalar factor of k.
            inline Quaternion scale(const Quaternion& q, const Scalar k);

            /// Returns the sum of two quaternions.
            inline Quaternion add(const Quaternion& q1, const Quaternion& q2);

            /// Returns the sum of two quaternions, resolving antipodality by flipping
            /// the sign of q2 if q1.q2 is negative. This operation is usually
            /// denoted as a circled + sign, forming the basis of the QLERP algorithm.
            /// See "Spherical Blend Skinning" (Kavan & Zara 2005) for more details.
            inline Quaternion addQlerp(const Quaternion& q1, const Quaternion& q2);

            // Note : the .inl file also define operator+ for quaternions
            // and operator * and / between quaternions and scalar.

            /// Decompose a given rotation Qin into a swing rotation and a twist rotation.
            /// Qswing is a rotation whose axis lies in the XY plane and Qtwist is a rotation about axis Z.
            /// such as Qin = Qswing * Qtwist
            /// If the rotation is already around axis z, Qswing will be set to identity
            /// and Qtwist equal to Qin
            inline void getSwingTwist(const Quaternion& in, Quaternion& swingOut, Quaternion& twistOut);

            /// Call std::isnormal on quaternion entries.
            template< typename Quaternion_ >
            inline bool checkInvalidNumbers(Eigen::Ref<const Quaternion_> q,
                                             const bool FAIL_ON_ASSERT = false) {
                return MatrixUtils::checkInvalidNumbers( q.coeffs(), FAIL_ON_ASSERT );
            }
        }

        // Use this macro in the public: section of a class
        // when declaring objects containing Vector or Matrices.
        // http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html
#define  RA_CORE_ALIGNED_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        /// Use this parameter for aligning structures with Eigen members in it
#define  RA_DEFAULT_ALIGN EIGEN_MAX_ALIGN_BYTES

    }
} // end namespace Ra::Core

#include <Core/Math/LinearAlgebra.inl>

#endif// RADIUMENGINE_VECTOR_HPP


#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <numeric>

#include "operationVector.h" // ceci va chercher le fichier qui contient les operations sur les vecteurs

using namespace std;
using namespace boost::numeric::ublas;

matrix<double> Beta(matrix<double> Obs, matrix<double> M, std::vector<double> pie) {

// 	Obs est la matrice des observations
// 	M est la matrice de transition
// 	pie est le vecteur de probabilite initiale

	const size_t m = Obs.size2(); // Nombre detat, de cases
	const size_t n = Obs.size1(); // Nombre dobservation, duree de la partie

//	matrix Alpha = matrix(NbLigne= n, NbCol= m);  Initialisation de la matric Alpha de tailler n*m
	matrix<double> Beta(n, m);
	matrix<double> bet(1,m);

	fill(bet, [&](int l, int c){
		return 1.0/m;
	});

	for (int i = 0; i < m; ++i) {

		Beta(n-1, i) = 0; // Si je comprend bien, la derniere ligne est la n-1 ?
	}

	double log1surc = log(sum(bet));
	matrix<double> Bet{1, m};

	matrix<double> Tligne = trans(ligne(Obs,n-1));
	Bet = prod(M,Tligne);
	Bet = trans(Bet);
	for (int i = 0; i < m; ++i) {

		Beta(n-2, i) = log(Bet(0,i))+log1surc;
	}

	log1surc = log1surc + log(sum(Bet));

	 Bet /= sum(Bet);

	//for (int i = 0; i < m; ++i) {

	//	Bet(0, i) = Bet(0,i)/sum(Bet);
	//}

	for (int i = 3; i < n+1 ; ++i) { // J'ai changer la partie "i < n, ++j" pour "i < n ; ++i"

		Tligne = trans(ligne(Obs,n-i+1));

		for (int j = 0; j < m ; ++j) {
			Tligne(j, 0) = Tligne(j, 0) * Bet(0, j);
		}

		Bet = prod(M, Tligne); // prod est le produit matriciel
		Bet = trans(Bet);

		for (int j = 0; j < m ; ++j) {
			Beta(n-i, j) = log(Bet(0, j)) + log1surc;
		}

		log1surc += log(sum(Bet));

		//for (int i = 0; i < m; ++i) {

		//	Bet(0, i) = Bet(0,i)/sum(Bet);
		//}

		 Bet /= sum(Bet);

	}

	return Beta;

}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with alternating least squares. We first creates factors and
 * then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

#include <mf/loss/nzsl.h>
#include <mf/loss/l2.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 1000000;
	double sigma = 1; // standard deviation
	double lambda = 1/sigma/sigma;
	mf_size_type r = 10;

	// parameters for ALS
	unsigned epochs = 20;
	AlsRegularizer regularizer = ALS_L2;
	typedef SumLoss<NzslLoss, L2Loss> Loss;
	typedef NzRmseLoss TestLoss;
	Loss loss((NzslLoss()), L2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = nnz/100;

	BalanceType type = BALANCE_L2;;
	BalanceMethod method = BALANCE_SIMPLE;

	// generate original factors by sampling from a normal(0,sigma) distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, r);
	DenseMatrixCM hIn(r, size2);
	generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
	generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

	// generate a sparse matrix by selecting random entries from the generated factors
	// and add small Gaussian noise
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	addRandom(v, random, boost::normal_distribution<>(0, 0.1));
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	v.sort();
	LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
	SparseMatrixCM vc;
	copyCm(v, vc);

	// create a test matrix (without noise)
	SparseMatrix vTest;
	generateRandom(vTest, testNnz, wIn, hIn, random);
	LOG4CXX_INFO(logger, "Test matrix: "
		<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

	// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
	DenseMatrix w(size1, r);
	DenseMatrixCM h(r, size2);
	generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
	generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

	// print the test loss
	LOG4CXX_INFO(logger, "Initial test loss: "
			<< testLoss(FactorizationData<>(vTest,w,h)));

	FactorizationData<> testJob(vTest,w,h);

	// initialize
	FactorizationData<> data(v, w, h, 1, &vc);
	Trace trace;
	// here add fields to Trace
	//trace.addField("balancing-type", type);
	//trace.addField("balancing-method", method);
	Timer t;

	// run ALS to try to reconstruct the original factors
	t.start();
//	alsNzsl(data, epochs, trace, lambda, regularizer, rescale);
	alsNzsl(data, epochs, trace, lambda, regularizer, type, method ,&testJob);
	t.stop();
	LOG4CXX_INFO(logger, "Total time: " << t);

	// print the test loss
	LOG4CXX_INFO(logger, "Final test loss: "
			<< testLoss(FactorizationData<>(vTest,w,h)));

	// write trace to an R file
	string typeString, methodString;
	switch (type) {
	case BALANCE_NONE:
		typeString = "None";
		break;
	case BALANCE_L2:
		typeString = "L2";
		break;
	case BALANCE_NZL2:
		typeString = "Nzl2";
		break;
	}
	switch (method) {
	case BALANCE_SIMPLE:
		methodString = "Simple";
		break;
	case BALANCE_OPTIMAL:
		methodString = "Optimal";
		break;
	}
	string filename = "/tmp/als-trace.R";
	LOG4CXX_INFO(logger, "Writing trace to " << filename);
	trace.toRfile(filename, "als");

	return 0;
}