// ***********************************************************************
// Code Created by James Michael Armstrong (https://github.com/BlazesRus)
// Latest Code Release at https://github.com/BlazesRus/BlazesRusSharedCode
// ***********************************************************************

#include <iostream>
#include "MediumDec.hpp"

using MediumDec = BlazesRusCode::MediumDec;
using VariableConversionFunctions = BlazesRusCode::VariableConversionFunctions;

#include <cmath>
#include "FloatingOperations.hpp"

#include <iostream>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>

namespace BlazesRusDebug
{
    /// <summary>
    /// Get the (n)th Root
    /// Code from https://rosettacode.org/wiki/Nth_root#C.23 (Derived from C version)
    /// </summary>
    /// <param name="value">The value to apply the root to.</param>
    /// <param name="n">The n value to apply with root.</param>
    /// <param name="precision">Precision level (smaller = more precise)</param>
    /// <returns></returns>
    static MediumDec NthRootV1(MediumDec value, int n)
    {//Accuracy is off for 4th root of 4 compared to calculator at (1.65625 compared to 1.4953487812212205419118989941409)
        MediumDec Prec = MediumDec(0, 10);
        MediumDec NegPrec = MediumDec(MediumDec::NegativeRep, 10);//-Prec;
        double PrecFl = DBL_EPSILON * 10;
        double NegPrecFL = -DBL_EPSILON * 10;

        MediumDec d, r = MediumDec::One;
        double dFL, rFL = 1.0;

        double valAsFloating = (double)value;
        int nMinus1 = n - 1;

        MediumDec powResult;
        double powResultFL;
        //if (n < 1 || (value < 0.0 && !(n & 1))) {
        //    return;//0.0 / 0.0; /* NaN */
        //}
        do {
            powResultFL = pow(rFL, nMinus1);
            powResult = MediumDec::Pow(r, nMinus1);
            dFL = valAsFloating / powResultFL - rFL;
            d = value / powResult - r;
            dFL /= n;
            d /= n;
            rFL += dFL;
            r += d;
            //std::cout << "PowerResult Values(double,MediumDec) =" << powResultFL << "," << powResult.ToString() << std::endl;
            //std::cout << "Floating Values(d,r) =" << dFL << "," << rFL << std::endl;
            //std::cout << "MediumDec Values(d,r) =" << d.ToString() << "," << r.ToString();
            std::cout << std::endl;
        } while (d >= PrecFl || d <= NegPrecFL);//while (d >= Prec || d <= NegPrec);//precision check condition
        return r;
    }

    /// <summary>
    /// Finds nTh Root of value based on https://stackoverflow.com/questions/9652549/self-made-pow-c code
    /// </summary>
    /// <param name="value">The value.</param>
    /// <param name="nValue">The nth value.</param>
    /// <param name="KPrecision">Precision level (larger = more precise)</param>
    /// <returns>MediumDec</returns>
    static MediumDec NthRootV4(MediumDec value, int n, int KPrecision=5)
    {
        int nMinus1 = n - 1;
        int nPlus1 = n + 1;
        const int LastElem = KPrecision-1;
        std::vector<MediumDec> x;
        x.push_back(MediumDec::One);
        MediumDec Temp;
        for (int XElem = 0; XElem < KPrecision; ++XElem)
        {
            Temp = (MediumDec::One / n) * (nMinus1 * x.back() + value / MediumDec::PowConstOp(x.back(), nMinus1));
            x.push_back(Temp);
        }
        return x.back();
    }

      /// <summary>
      /// Get the (n)th Root
      /// Code based mostly from https://rosettacode.org/wiki/Nth_root#C.23
      /// </summary>
      /// <param name="n">The n value to apply with root.</param>
      /// <returns></returns>
      static MediumDec NthRootV5(MediumDec targetValue, int n, MediumDec& Precision=MediumDec::FiveBillionth)
      {
          int nMinus1 = n - 1;
          MediumDec x[2] = { (MediumDec::One / n) * ((nMinus1 * targetValue) + (targetValue / MediumDec::Pow(targetValue, nMinus1))), targetValue };
          while (MediumDec::Abs(x[0] - x[1]) > Precision)
          {
              x[1] = x[0];
              x[0] = (MediumDec::One / n) * ((nMinus1 * x[1]) + (targetValue / MediumDec::Pow(x[1], nMinus1)));
          }
          return x[0];
      }

      /// <summary>
      /// Log Base 10 of Value
      /// </summary>
      /// <param name="Value">The value.</param>
      /// <returns>MediumDec</returns>
      static MediumDec Log10(MediumDec Value)
      {
          return MediumDec::Ln(Value) / MediumDec::LN10;
      }

      /// <summary>
      /// Log Base 10 of Value
      /// </summary>
      /// <param name="Value">The value.</param>
      /// <returns>MediumDec</returns>
      static MediumDec Log10(int Value)
      {
          return MediumDec::Ln(Value) / MediumDec::LN10;
      }

      /// <summary>
      /// Log with Base of BaseVal of Value
      /// Based on http://home.windstream.net/okrebs/page57.html
      /// </summary>
      /// <param name="Value">The value.</param>
      /// <param name="BaseVal">The base of Log</param>
      /// <returns>MediumDec</returns>
      static MediumDec Log(MediumDec Value, MediumDec BaseVal)
      {
          return Log10(Value) / Log10(BaseVal);
      }

      /// <summary>
      /// Log with Base of BaseVal of Value
      /// Based on http://home.windstream.net/okrebs/page57.html
      /// </summary>
      /// <param name="Value">The value.</param>
      /// <param name="BaseVal">The base of Log</param>
      /// <returns>MediumDec</returns>
      static MediumDec Log(MediumDec Value, int BaseVal)
      {
          return Log10(Value) / Log10(BaseVal);
      }

    /// <summary>
    /// Natural log
    /// </summary>
    /// <param name="value">The target value.</param>
    static MediumDec Ln(MediumDec value)
    {
        //if (value <= 0) {}else//Error if equal or less than 0
        if (value == MediumDec::One)
            return MediumDec::Zero;
        else if (value.IntValue < 2)//Threshold between 0 and 2 based on Taylor code series from https://stackoverflow.com/questions/26820871/c-program-which-calculates-ln-for-a-given-variable-x-without-using-any-ready-f
        {//This section gives correct answer
            MediumDec threshold = "0.00005";  // set this to whatever threshold you want
            MediumDec base = value - 1;        // Base of the numerator; exponent will be explicit
            int den = 1;              // Denominator of the nth term
            bool posSign = true;             // Used to swap the sign of each term
            MediumDec term = base;       // First term
            MediumDec prev = 0;          // Previous sum
            MediumDec result = term;     // Kick it off

            while (MediumDec::Abs(prev - result) > threshold) {
                den++;
                posSign = !posSign;
                term *= base;
                prev = result;
                if (posSign)
                    result += term / den;
                else
                    result -= term / den;
            }

            return result;
        }
        else//Returns a positive value(http://www.netlib.org/cephes/qlibdoc.html#qlog)
        {//Increasing iterations brings closer to accurate result
            MediumDec W = (value - 1) / (value + 1);
            MediumDec TotalRes = W;
#if(_DEBUG)
            MediumDec AddRes = MediumDec::Maximum;
            int WPow;
            for(WPow=3; AddRes>MediumDec::JustAboveZero;WPow+=2)
            {
                AddRes = MediumDec::PowRef(W, WPow) / WPow;
                TotalRes += AddRes;
            }
            TotalRes *= 2;
            std::cout << "Ln(" << value.ToString() << ") = " << TotalRes.ToString() << " FloatingResult:" << log((double)value) <<" WPow:"<< WPow <<" AddRes:"<<AddRes.ToString()<< std::endl;
            for (int AfterPow = WPow+10; WPow < AfterPow; WPow += 2)//Testing to see if AddRes fluctuates after getting small(to optimize to potentially get more accurate loop iteration number)
            {
                AddRes = MediumDec::PowRef(W, WPow) / WPow;
                std::cout << "(AfterPow)" << AddRes.ToString() << " at WPow Val of " << WPow << std::endl;
            }
            return TotalRes;
#else
            for (int WPow = 3; WPow <63; WPow += 2)
            {
                TotalRes += MediumDec::PowRef(W, WPow) / WPow;
            }
#endif
            return TotalRes * 2;
        }
    }

    /// <summary>
    /// Natural log (Equivalent to Log_E(value))
    /// </summary>
    /// <param name="value">The target value.</param>
    /// <param name="threshold">The threshold value for when value is between 0 and 2.</param>
    /// <returns>BlazesRusCode::MediumDec</returns>
    static MediumDec LnRef(MediumDec& value, MediumDec threshold)
    {
        //if (value <= 0) {}else//Error if equal or less than 0
        if (value == MediumDec::One)
            return MediumDec::Zero;
        else if (value.IntValue < 2)//Threshold between 0 and 2 based on Taylor code series from https://stackoverflow.com/questions/26820871/c-program-which-calculates-ln-for-a-given-variable-x-without-using-any-ready-f
        {//This section gives accurate answer
            MediumDec base = value - 1;        // Base of the numerator; exponent will be explicit
            int den = 1;              // Denominator of the nth term
            bool posSign = true;             // Used to swap the sign of each term
            MediumDec term = base;       // First term
            MediumDec prev = 0;          // Previous sum
            MediumDec result = term;     // Kick it off

            while (MediumDec::Abs(prev - result) > threshold) {
                den++;
                posSign = !posSign;
                term *= base;
                prev = result;
                if (posSign)
                    result += term / den;
                else
                    result -= term / den;
            }

            return result;
        }
        else//Returns a positive value(http://www.netlib.org/cephes/qlibdoc.html#qlog)
        {//Increasing iterations brings closer to accurate result(Larger numbers need more iterations to get accurate level of result)
            MediumDec W = (value - 1) / (value + 1);
            MediumDec TotalRes = W;
            MediumDec AddRes;
            //for (int WPow = 3; AddRes > MediumDec::JustAboveZero; WPow += 2)
            int WPow = 3;
            do
            {
                AddRes = MediumDec::PowRef(W, WPow) / WPow;
                TotalRes += AddRes; WPow += 2;
            } while (AddRes > MediumDec::JustAboveZero);
            return TotalRes * 2;
        }
    }

    /// <summary>
    /// Natural log (Equivalent to Log_E(value))
    /// </summary>
    /// <param name="value">The target value.</param>
    static MediumDec Ln(MediumDec value, MediumDec threshold)
    {
        return LnRef(value, threshold);
    }

    /// <summary>
    /// Natural log TestVersion(Equivalent to Log_E(value))
    /// </summary>
    /// <param name="value">The target value.</param>
    /// <returns>double</returns>
    static MediumDec LnTestRef(double& fvalue, MediumDec& value)
    {
        //if (value <= 0) {}else//Error if equal or less than 0
        if (value==MediumDec::One)
            return MediumDec::Zero;
        if (value.IntValue==0)//Returns a negative number derived from (http://www.netlib.org/cephes/qlibdoc.html#qlog)
        {
            double fW = (fvalue - 1.0);
            MediumDec W = (value - 1);
            std::cout << "--Floating W Numerator:" << fW << " vs " << W.ToString() << "--" << std::endl;
            double fDenom = (fvalue + 1.0);
            fW /= fDenom;//(-0.5)/(1.5)=-1/3
            MediumDec Denom = (value + 1);
            std::cout << "--Floating W Denom:" << fDenom << " vs " << Denom.ToString() << "--" << std::endl;
            W /= Denom;
            double fTotalRes = fW;
            fW *= -1;
            double fLastPow = fW;
            double fWSquared = fW * fW;
            double fAddRes;
            int WPow = 3;

            MediumDec TotalRes = W;
            W.SwapNegativeStatus();
            MediumDec LastPow = W;
            MediumDec WSquared = W * W;
            MediumDec AddRes;
            std::cout << "--Floating W:" << fW << " vs " << W.ToString() << "--" << std::endl;
            do
            {
                fLastPow *= fWSquared;
                LastPow *= WSquared;
                //std::cout << "-----Floating LastPow:" << fLastPow << " vs " << LastPow.ToString() << "--" << std::endl;

                fAddRes = fLastPow / WPow;
                AddRes = LastPow / WPow;
                //std::cout << "-----Floating AddRes:" << fAddRes << " vs " << AddRes.ToString() << "--" << std::endl;

                fTotalRes -= fAddRes;
                TotalRes -= AddRes;

                WPow += 2;
            } while (AddRes > MediumDec::JustAboveZero);//Total Result should be -0.346573590279972654708616060729088284037750067180127627060340004746696810984847357802931663498209344
            return TotalRes * 2;//Should result in -0.693147180559
        }
        else if(value.IntValue==1)
        {
            MediumDec threshold = MediumDec::FiveMillionth;
            MediumDec base = value - 1;        // Base of the numerator; exponent will be explicit
            int den = 2;              // Denominator of the nth term
            bool posSign = true;             // Used to swap the sign of each term
            MediumDec term = base;       // First term
            MediumDec prev;          // Previous sum
            MediumDec result = term;     // Kick it off

            do
            {
                posSign = !posSign;
                term *= base;
                prev = result;
                if (posSign)
                    result += term / den;
                else
                    result -= term / den;
                den++;
            } while (MediumDec::Abs(prev - result) > threshold);

            return result;
        }
        else//Returns a positive value(http://www.netlib.org/cephes/qlibdoc.html#qlog)
        {//Increasing iterations brings closer to accurate result(Larger numbers need more iterations to get accurate level of result)
            MediumDec TotalRes = (value - 1) / (value + 1);//W;
            MediumDec LastPow = TotalRes;
            MediumDec WSquared = TotalRes * TotalRes;
            MediumDec AddRes;
            int WPow = 3;
            do
            {
                LastPow *= WSquared;
                AddRes = LastPow / WPow;//MediumDec::PowRef(W, WPow) / WPow;
                TotalRes += AddRes; WPow += 2;
            } while (AddRes > MediumDec::JustAboveZero);
            return TotalRes * 2;
        }
    }

    /// <summary>
    /// Natural log (Equivalent to Log_E(value))
    /// </summary>
    /// <param name="value">The target value.</param>
    static MediumDec LnTest(double fvalue, MediumDec value)
    {
        return LnTestRef(fvalue, value);
    }
}


#define _USE_MATH_DEFINES
#include <iostream>
#include <cmath>

using namespace std;

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

using namespace Eigen;

//本程式演示了 Eigen 幾何模組的使用方法

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

	// Eigen/Geometry 模組提供了各種旋轉和平移的表示方法
	// 3D 旋轉矩陣直接使用 Matrix3d 或 Matrix3f
	Matrix3d rotation_matrix = Matrix3d::Identity();
	// 旋轉向量使用 AngleAxis，它底層不是Matrix，但運算可以當作矩陣(因為運算子超載)
	AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 軸旋轉 45 度
	cout.precision(3);
	cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()轉換成矩陣
	// 也可以直接賦值
	rotation_matrix = rotation_vector.toRotationMatrix();
	// 用 AngleAxis 可以進行座標變換
	Vector3d v(1, 0, 0);
	Vector3d v_rotated = rotation_vector * v;
	cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
	// 或者用旋轉矩陣
	v_rotated = rotation_matrix * v;
	cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

	// 尤拉角: 可以將旋轉矩陣直接轉換成尤拉角
	Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX順序，即yaw-pitch-roll順序
	cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

	// 歐氏變換矩陣使用 Eigen::Isometry
	Isometry3d T = Isometry3d::Identity();                // 雖然稱為3d，實質上是4＊4的矩陣
	T.rotate(rotation_vector);                            // 按照rotation_vector進行旋轉
	T.pretranslate(Vector3d(1, 3, 4));                    // 把平移向量設成(1,3,4)
	cout << "Transform matrix = \n" << T.matrix() << endl;

	// 用變換矩陣進行座標變換
	Vector3d v_transformed = T * v;                              // 相當於R*v+t
	cout << "v tranformed = " << v_transformed.transpose() << endl;

	// 對於仿射和射影變換，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

	// 四元數
	// 可以直接把AngleAxis賦值给四元數，反之亦然
	Quaterniond q = Quaterniond(rotation_vector);
	cout << "quaternion from rotation vector = " << q.coeffs().transpose()
		<< endl;   // 請注意coeffs的順序是(x,y,z,w),w為實部，前三者為虛部
   // 也可以把旋轉矩陣賦給它
	q = Quaterniond(rotation_matrix);
	cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
	// 使用四元數旋轉一个向量，使用超載的乘法即可
	v_rotated = q * v; // 注意數學上是qvq^{-1}
	cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
	// 用常規向量乘法表示，則應該如下計算
	cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

	return 0;
}


// This program is free software: you can use, modify and/or redistribute it
// under the terms of the simplified BSD License. You should have received a
// copy of this license along this program. If not, see
// <http://www.opensource.org/licenses/bsd-license.html>.
//
// Copyright (C) 2021, Thibaud Briand <briand.thibaud@gmail.com>
// Copyright (C) 2021, Jonathan Vacher <jonathan.vacher@einstein.yu.edu>
// All rights reserved.

#include <stdlib.h>
#include <math.h>
#include "Eigen/Dense"
#include <Eigen/Eigenvalues>

using namespace Eigen;
using namespace std;

// Compute the color covariance matrix of a color image (Equation 97)
// The mean of each channel of the input is assumed to be 0
void compute_covariance(float *im, float C[3][3], int N)
{
  int i, l;
  MatrixXf U(3,N);

  // put input data in a matrix
  for(l = 0; l < 3; l++)
    for(i = 0; i < N; i++)
      U(l,i) = im[i + l*N];

  // compute the color covariance matrix (Equation 97)
  Matrix3f cov = U*(U.transpose())/N;

  // put the covariance matrix in 3x3 array
  for(i = 0; i < 3; i++)
    for(l = 0; l < 3; l++)
      C[i][l] = cov(i,l);
}

// Compute the eigenvector decomposition of a symmetric matrix
// We have A = V*diag(d)*V' where:
// 1) V is an orthogonal matrix containing the eigenvectors
// 2) d contains the eigenvalues
void eigen_decomposition(const float A[3][3], float V[3][3], float d[3])
{
  int i, j;
  Matrix3f C;

  // put array A in a matrix C
  for(i = 0; i < 3; i++)
    for(j = 0; j < 3; j++)
      C(i,j) = A[i][j];

  // eigen solver
  SelfAdjointEigenSolver<Matrix3f> eigensolver(C);

  // eigenvectors
  Matrix3f P = eigensolver.eigenvectors();
  for(i = 0; i < 3; i++)
    for(j = 0; j < 3; j++)
      V[i][j] = P(i,j);

  // eigenvalues
  Vector3f E = eigensolver.eigenvalues();
  for(i = 0; i < 3; i++)
    d[i] = E(i);
}

// Apply the direct PCA transform (Equation 99)
// out = diag(d)^{-1/2}*V'*in
// The mean of each channel of the input is assumed to be 0
void apply_pca(float *out, const float *in, const float V[3][3],
               const float d[3], int N)
{
  int i, l;

  // put data in matrices
  MatrixXf U(3,N);
  for(l = 0; l < 3; l++)
    for(i = 0; i < N; i++)
      U(l,i) = in[i + l*N];

  Matrix3f V2;
  for(l = 0; l < 3; l++)
    for(i = 0; i < 3; i++)
      V2(i,l) = V[i][l];

  // compute d^{-1/2}
  // eigenvalues smaller than 1e-2 are set to 0 to avoid instability
  // this is useless when the function sanity_check was used to handle
  // input images with strange content
  Vector3f d2;
  for(l = 0; l < 3; l++)
    d2(l) = (d[l] < 1e-2) ? 0 : 1.0/sqrt(d[l]);

  // apply direct PCA transform (Equation 82)
  // out = diag(d)^{-1/2}*V'*in
  U = (d2.asDiagonal())*(V2.transpose())*U;

  // put the computed matrix in the output array
  for(l = 0; l < 3; l++)
    for(i = 0; i < N; i++)
      out[i + l*N] = U(l,i);
}

// Apply the inverse PCA transform (Equation 100)
// out = V*diag(d)^{1/2}*in
// The mean of each channel of the input is assumed to be 0
void apply_inverse_pca(float *out, const float *in, const float V[3][3],
                       const float d[3], int N)
{
  int i, l;

  // put data in matrices
  MatrixXf tildeU(3,N);
  for(l = 0; l < 3; l++)
    for(i = 0; i < N; i++)
      tildeU(l,i) = in[i + l*N];

  Matrix3f V2;
  for(l = 0; l < 3; l++)
    for(i = 0; i < 3; i++)
      V2(i,l) = V[i][l];

  // compute d^{1/2}
  Vector3f d2;
  for(l = 0; l < 3; l++)
    d2(l) = sqrt(d[l]);

  // apply inverse PCA transform (Equation 83)
  // out = V*diag(d)^{1/2}*in
  tildeU = V2*(d2.asDiagonal())*tildeU;

  // put the computed matrix in the output array
  for(l = 0; l < 3; l++)
    for(i = 0; i < N; i++)
      out[i + l*N] = tildeU(l,i);
}


#include "WindingNumbers.h"
#include "SPlisHSPlasH/Common.h"
#include <Eigen/Dense>

#define _USE_MATH_DEFINES
#include "math.h"

using namespace Utilities;
using namespace SPH;
using namespace Eigen;

Real WindingNumbers::computeGeneralizedWindingNumber(const Vector3r& p_, const Vector3r& a_, const Vector3r& b_, const Vector3r& c_)
{
 	const Vector3r& a = a_ - p_;
 	const Vector3r& b = b_ - p_;
 	const Vector3r& c = c_ - p_;

 	const Real normA = a.norm();
 	const Real normB = b.norm();
 	const Real normC = c.norm();

	Matrix3r A;
	A.row(0) = a;
	A.row(1) = b;
	A.row(2) = c;
 	const Real det = A.determinant();
 	const Real divisor = normA*normB*normC + (a.dot(b))*normC + (b.dot(c))*normA + (c.dot(a))*normB;

 	static const Real tau = 2.0 *  M_PI;
 	return std::atan2(det,divisor) / tau; // Only divide by 2*pi instead of 4*pi because there was a 2 out front
}

Real WindingNumbers::computeGeneralizedWindingNumber(const Vector3r& p, const TriangleMesh& mesh)
{
 	const unsigned int *faces = mesh.getFaces().data();
 	const unsigned int nFaces = mesh.numFaces();
	const Vector3r *v = mesh.getVertices().data();

 	// compute generalized winding number
 	Real w_p = 0;
 	#pragma omp parallel for reduction (+: w_p)
 	for (int idx = 0; idx < static_cast<int>(nFaces); idx ++)
 	{
 		const Vector3r& a = v[faces[idx*3+0]];
 		const Vector3r& b = v[faces[idx*3+1]];
 		const Vector3r& c = v[faces[idx*3+2]];

 		w_p += computeGeneralizedWindingNumber(p, a, b, c);
 	}

 	return w_p;
}


#ifndef MATHTOOLBOX_DATA_NORMALIZATION_HPP
#define MATHTOOLBOX_DATA_NORMALIZATION_HPP

#include <Eigen/Core>

namespace mathtoolbox
{
    class DataNormalizer
    {
    public:
        /// \param data_points An n-by-m matrix representing m data points lying in an n-dimensional space.
        DataNormalizer(const Eigen::MatrixXd& data_points) : m_original_data_points(data_points)
        {
            const int num_dims        = m_original_data_points.rows();
            const int num_data_points = m_original_data_points.cols();

            assert(num_dims > 0);
            assert(num_data_points > 0);

            m_mean  = Eigen::VectorXd(num_dims);
            m_stdev = Eigen::VectorXd(num_dims);

            for (int dim = 0; dim < num_dims; ++dim)
            {
                const Eigen::VectorXd one_dim_data = m_original_data_points.row(dim);

                m_mean(dim) = one_dim_data.mean();
                m_stdev(dim) =
                    std::sqrt((one_dim_data - Eigen::VectorXd::Constant(num_data_points, m_mean(dim))).squaredNorm() /
                              static_cast<double>(num_data_points));
            }

            m_normalized_data_points = Normalize(m_original_data_points);
        }

        /// \brief Get the normalized data points.
        const Eigen::MatrixXd& GetNormalizedDataPoints() const { return m_normalized_data_points; }

        /// \brief Normalize a new set of data points by the same transformation as the originally provided data points.
        Eigen::MatrixXd Normalize(const Eigen::MatrixXd& data_points) const
        {
            const int num_dims        = data_points.rows();
            const int num_data_points = data_points.cols();

            Eigen::MatrixXd normalized_data_points(num_dims, num_data_points);
            for (int dim = 0; dim < num_dims; ++dim)
            {
                constexpr double epsilon = 1e-32;
                const double     scale   = 1.0 / std::max(m_stdev(dim), epsilon);
                const auto       offset  = Eigen::VectorXd::Constant(num_data_points, m_mean(dim));

                normalized_data_points.row(dim) = scale * (data_points.row(dim) - offset.transpose());
            }

            return normalized_data_points;
        }

        /// \brief Denormalize a new set of data points that is represented as the normalized form.
        Eigen::MatrixXd Denormalize(const Eigen::MatrixXd& normalized_data_points) const
        {
            const int num_dims        = normalized_data_points.rows();
            const int num_data_points = normalized_data_points.cols();

            Eigen::MatrixXd data_points(num_dims, num_data_points);
            for (int dim = 0; dim < num_dims; ++dim)
            {
                constexpr double epsilon = 1e-32;
                const double     scale   = std::max(m_stdev(dim), epsilon);
                const auto       offset  = Eigen::VectorXd::Constant(num_data_points, m_mean(dim));

                data_points.row(dim) = scale * normalized_data_points.row(dim) + offset.transpose();
            }

            return data_points;
        }

        /// \brief Get internal parameters about the means.
        const Eigen::VectorXd& GetMean() const { return m_mean; }

        /// \brief Get internal parameters about the standard deviations.
        const Eigen::VectorXd& GetStdev() const { return m_stdev; }

    private:
        const Eigen::MatrixXd m_original_data_points;

        Eigen::MatrixXd m_normalized_data_points;
        Eigen::VectorXd m_mean;
        Eigen::VectorXd m_stdev;
    };
} // namespace mathtoolbox

#endif // MATHTOOLBOX_DATA_NORMALIZATION_HPP


#pragma once

#ifdef BOOST_UBLAS_TYPE_CHECK
#	undef BOOST_UBLAS_TYPE_CHECK
#endif
#define BOOST_UBLAS_TYPE_CHECK 0
#ifndef _USE_MATH_DEFINES
#	define _USE_MATH_DEFINES
#endif

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <vector>
#include <boost/assert.hpp>
#include <stdexcept>
#include <cassert>

/*
	Finds the coefficients of a polynomial p(x) of degree n that fits the data,
	p(x(i)) to y(i), in a least squares sense. The result p is a row vector of
	length n+1 containing the polynomial coefficients in incremental powers.

	param:
		oX				x axis values
		oY				y axis values
		nDegree			polynomial degree including the constant

	return:
		coefficients of a polynomial starting at the constant coefficient and
		ending with the coefficient of power to nDegree. C++0x-compatible
		compilers make returning locally created vectors very efficient.

*/
template<typename T>
std::vector<T> polyfit( const std::vector<T>& oX, const std::vector<T>& oY, int nDegree )
{
	using namespace boost::numeric::ublas;

	if ( oX.size() != oY.size() )
		throw std::invalid_argument( "X and Y vector sizes do not match" );

	// more intuative this way
	nDegree++;

	size_t nCount =  oX.size();
	matrix<T> oXMatrix( nCount, nDegree );
	matrix<T> oYMatrix( nCount, 1 );

	// copy y matrix
	for ( size_t i = 0; i < nCount; i++ )
	{
		oYMatrix(i, 0) = oY[i];
	}

	// create the X matrix
	for ( size_t nRow = 0; nRow < nCount; nRow++ )
	{
		T nVal = 1.0f;
		for ( int nCol = 0; nCol < nDegree; nCol++ )
		{
			oXMatrix(nRow, nCol) = nVal;
			nVal *= oX[nRow];
		}
	}

	// transpose X matrix
	matrix<T> oXtMatrix( trans(oXMatrix) );
	// multiply transposed X matrix with X matrix
	matrix<T> oXtXMatrix( prec_prod(oXtMatrix, oXMatrix) );
	// multiply transposed X matrix with Y matrix
	matrix<T> oXtYMatrix( prec_prod(oXtMatrix, oYMatrix) );

	// lu decomposition
	permutation_matrix<int> pert(oXtXMatrix.size1());
	const std::size_t singular = lu_factorize(oXtXMatrix, pert);
	// must be singular
	BOOST_ASSERT( singular == 0 );

	// backsubstitution
	lu_substitute(oXtXMatrix, pert, oXtYMatrix);

	// copy the result to coeff
	return std::vector<T>( oXtYMatrix.data().begin(), oXtYMatrix.data().end() );
}

/*
	Calculates the value of a polynomial of degree n evaluated at x. The input
	argument pCoeff is a vector of length n+1 whose elements are the coefficients
	in incremental powers of the polynomial to be evaluated.

	param:
		oCoeff			polynomial coefficients generated by polyfit() function
		oX				x axis values

	return:
		Fitted Y values. C++0x-compatible compilers make returning locally
		created vectors very efficient.
*/
template<typename T>
std::vector<T> polyval( const std::vector<T>& oCoeff, const std::vector<T>& oX )
{
	size_t nCount =  oX.size();
	size_t nDegree = oCoeff.size();
	std::vector<T>	oY( nCount );

	for ( size_t i = 0; i < nCount; i++ )
	{
		T nY = 0;
		T nXT = 1;
		T nX = oX[i];
		for ( size_t j = 0; j < nDegree; j++ )
		{
			// multiply current x by a coefficient
			nY += oCoeff[j] * nXT;
			// power up the X
			nXT *= nX;
		}
		oY[i] = nY;
	}

	return oY;
}


#include <cmath>

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

namespace InterPose {
namespace Utils {

  // -1 <= cos_theta <= 1, 0 <= theta <= pi
  const double EPS_to_0 = 1e-5;
  const double EPS_to_1 = 1.0 - 1e-5;

  template <typename TYPE>
  Eigen::Matrix<TYPE,3,3> log(const Eigen::Matrix<TYPE,3,3>& R, TYPE& theta) {
    Eigen::Matrix<TYPE,3,3> omega_x;

    const TYPE cos_theta = (R.trace() - 1.0)/2.0;

    if(cos_theta < -EPS_to_1) { // need to handle theta = pi
      Eigen::Matrix<TYPE,3,3> R_plus_I = R + Eigen::Matrix<TYPE,3,3>::Identity();
      int _col = -1;
      TYPE longest_col_length = -1.0;
      for(int c = 0; c < 3; ++c) {
        TYPE length = R_plus_I.col(c).norm();
        if(length > longest_col_length) {
          _col = c;
          longest_col_length = length;
        }

        theta = M_PI;
        const Eigen::Matrix<TYPE,3,1> omega = (M_PI/longest_col_length)
                                          * Eigen::Matrix<TYPE,3,1>(R_plus_I(0,_col), R_plus_I(1,_col), R_plus_I(2,_col));
        omega_x(0,0) = 0.0;       omega_x(0,1) = -omega(2); omega_x(0,2) = omega(1);
        omega_x(1,0) = omega(2);  omega_x(1,1) = 0;         omega_x(1,2) = -omega(0);
        omega_x(2,0) = -omega(1); omega_x(2,1) = omega(0);  omega_x(2,2) = 0.0;
      }
    }
    else if (cos_theta > EPS_to_1) { // need to handle theta = 0
      const TYPE _theta = std::acos((R.trace() - 1.0)/2.0);
      const Eigen::Matrix<TYPE,3,3> _omega_x = 0.5*(1.0 + _theta*_theta/6.0)*(R - R.transpose()); // log(R)

      omega_x = _omega_x;
      theta = _theta;
    }
    else { // usual case
      const TYPE _theta = std::acos(cos_theta);
      const Eigen::Matrix<TYPE,3,3> _omega_x = _theta*(R - R.transpose())/(2.0*std::sin(_theta)); // log(R)

      omega_x = _omega_x;
      theta = _theta;
    }

    return omega_x;
  }

  template <typename TYPE>
  Eigen::Matrix<TYPE,4,4> exp(const Eigen::Matrix<TYPE,3,1>& w, const Eigen::Matrix<TYPE,3,1>& t, const TYPE& theta) {
    Eigen::Matrix<TYPE,4,4> T = Eigen::Matrix4d::Identity();
    Eigen::Matrix<TYPE,3,3> w_x;
    w_x(0,0) = 0.0;   w_x(0,1) = -w(2); w_x(0,2) = w(1);
    w_x(1,0) = w(2);  w_x(1,1) = 0;     w_x(1,2) = -w(0);
    w_x(2,0) = -w(1); w_x(2,1) = w(0);  w_x(2,2) = 0.0;

    if(theta < EPS_to_0) {
      Eigen::Matrix<TYPE,3,3> e_w_x = Eigen::Matrix<TYPE,3,3>::Identity();
      T.block(0,0,3,3) = e_w_x;
      T.block(0,3,3,1) = e_w_x * t;
    }
    else {
      Eigen::Matrix<TYPE,3,3> e_w_x = Eigen::Matrix<TYPE,3,3>::Identity()
                              + (std::sin(theta)/theta)*w_x
                              + ((1.0-std::cos(theta))/(theta*theta))*w_x*w_x;

      Eigen::Matrix<TYPE,3,3> V = Eigen::Matrix<TYPE,3,3>::Identity()
                          + ((1.0-std::cos(theta))/(theta*theta))*w_x
                          + ((theta - std::sin(theta))/(theta*theta*theta))*w_x*w_x;

      Eigen::Matrix<TYPE,3,1> _t = V * t;

      T.block(0,0,3,3) = e_w_x;
      T.block(0,3,3,1) = _t;
    }

    return T;
  }
}
}


#include <Eigen/Geometry>
#include <Eigen/Jacobi>
#include <Eigen/SVD>
//#include <limits>
#ifndef EIGEN_PINV_HPP
#define EIGEN_PINV_HPP
namespace EIGEN_PINV
{
// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html )
// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
Eigen::MatrixXd pinv( const Eigen::MatrixXd &b, double rcond )
{
  // TODO: Figure out why it wants fewer rows than columns
  // if ( a.rows()<a.cols() )
  // return false;
  bool flip = false;
  Eigen::MatrixXd a;
  if( a.rows() < a.cols() )
  {
    a = b.transpose();
    flip = true;
  }
  else
    a = b;
  // SVD
  Eigen::JacobiSVD<Eigen::MatrixXd> svdA;
  svdA.compute( a, Eigen::ComputeFullU | Eigen::ComputeThinV );
  Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType vSingular = svdA.singularValues();
  // Build a diagonal matrix with the Inverted Singular values
  // The pseudo inverted singular matrix is easy to compute :
  // is formed by replacing every nonzero entry by its reciprocal (inversing).
  Eigen::VectorXd vPseudoInvertedSingular( svdA.matrixV().cols() );

  for (int iRow=0; iRow<vSingular.rows(); iRow++)
  {
    if ( fabs(vSingular(iRow)) <= rcond )
    {
      vPseudoInvertedSingular(iRow)=0.;
    }
    else
      vPseudoInvertedSingular(iRow)=1./vSingular(iRow);
  }
  // A little optimization here
  Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block( 0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols() );
  // Pseudo-Inversion : V * S * U'
  Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
  if( flip )
  {
    a = a.transpose();
    a_pinv = a_pinv.transpose();
  }
  return a_pinv;
}
}
#endif // EIGEN_PINV_HPP


#include <random>
#include <vector>
#include <algorithm>
#include <iterator>

#include <boost/geometry/geometry.hpp>

using namespace boost::geometry;

template
<
    typename OutPoint,
    typename InPoint,
    typename Generator
>
OutPoint sample_triangle_uniform(const InPoint& p1, const InPoint& p2, const InPoint& p3, Generator& gen)
{
    std::uniform_real_distribution<> dis(0, 1);
    double r1 = dis(gen);
    double r2 = dis(gen);
    return make<OutPoint>(
        (1-std::sqrt(r1))*get<0>(p1)+
        (std::sqrt(r1)*(1-r2))*get<0>(p2)+
        (r2*std::sqrt(r1))*get<0>(p3),
        (1-std::sqrt(r1))*get<1>(p1)+
        (std::sqrt(r1)*(1-r2))*get<1>(p2)+
        (r2*std::sqrt(r1))*get<1>(p3));
}

template
<
    typename Polygon,
    typename Collection
>
void sample_polygon_uniform(const Polygon& p, std::size_t num, Collection& out)
{
    typedef typename point_type<Polygon>::type point_type;
    typedef typename area_result<Polygon>::type area_type;
    typedef typename boost::range_value<Collection>::type out_point_type;
    typename ring_type<const Polygon>::type& ring = exterior_ring(p);
    const std::size_t length = boost::size(ring);
    std::vector<typename area_result<Polygon>::type> cumulative_area;
    cumulative_area.reserve(length-2);
    cumulative_area.push_back(0);
    area_type cum_sum = 0;
    for(std::size_t i = 1;i<length-2;++i) {
        cum_sum += triangle_double_area<area_type>(*boost::begin(ring), *(boost::begin(ring)+i), *(boost::begin(ring)+i+1));
        cumulative_area.push_back(cum_sum);
    }

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(0, 1);
    for(std::size_t i = 0; i<num; ++i) {
        area_type r = boost::numeric_cast<area_type>(dis(gen))*cum_sum;
        std::size_t t = std::distance(cumulative_area.begin(),
           std::lower_bound(cumulative_area.begin(),cumulative_area.end(), r));
        *std::back_inserter(out) = sample_triangle_uniform<out_point_type>(*boost::begin(ring), *(boost::begin(ring)+t), *(boost::begin(ring)+t+1),gen);
    }
}


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

// The original file belongs to MSCKF_VIO (https://github.com/KumarRobotics/msckf_vio/)
// Some changes have been made to use it in orcvio

#ifndef MATH_UTILS_HPP
#define MATH_UTILS_HPP

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

namespace orcvio {

/*
 *  @brief Create a skew-symmetric matrix from a 3-element vector.
 *  @note Performs the operation:
 *  w   ->  [  0 -w3  w2]
 *          [ w3   0 -w1]
 *          [-w2  w1   0]
 */
inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) {
  Eigen::Matrix3d w_hat;
  w_hat(0, 0) = 0;
  w_hat(0, 1) = -w(2);
  w_hat(0, 2) = w(1);
  w_hat(1, 0) = w(2);
  w_hat(1, 1) = 0;
  w_hat(1, 2) = -w(0);
  w_hat(2, 0) = -w(1);
  w_hat(2, 1) = w(0);
  w_hat(2, 2) = 0;
  return w_hat;
}
//     /**
//      * @brief Skew-symmetric matrix from a given 3x1 vector
//      *
//      * This is based on equation 6 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf):
//      * \f{align*}{
//      *  \lfloor\mathbf{v}\times\rfloor =
//      *  \begin{bmatrix}
//      *  0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0
//      *  \end{bmatrix}
//      * @f}
//      *
//      * @param[in] w 3x1 vector to be made a skew-symmetric
//      * @return 3x3 skew-symmetric matrix
//      */
// template<typename Der>
//     Eigen::Matrix<typename Der::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Der> &w) {
//         static_assert(Der::RowsAtCompileTime == 3, "3 vector");
//         static_assert(Der::ColsAtCompileTime == 1, "a vector");
//         Eigen::Matrix<typename Der::Scalar, 3, 3> w_x;
//         w_x << 0, -w(2, 0), w(1,0),
//                 w(2, 0), 0, -w(0, 0),
//                 -w(1, 0), w(0, 0), 0;
//         return w_x;
//     }

/*
 * @brief Normalize the given quaternion to unit quaternion.
 */
inline void quaternionNormalize(Eigen::Vector4d& q) {
  double norm = q.norm();
  q = q / norm;
  return;
}

/*
 * @brief Perform q1 * q2.
 *
 *    Format of q1 and q2 is as [x,y,z,w]
 */
inline Eigen::Vector4d quaternionMultiplication(
    const Eigen::Vector4d& q1,
    const Eigen::Vector4d& q2) {
  Eigen::Matrix4d L;

  // Hamilton
  L(0, 0) =  q1(3); L(0, 1) = -q1(2); L(0, 2) =  q1(1); L(0, 3) =  q1(0);
  L(1, 0) =  q1(2); L(1, 1) =  q1(3); L(1, 2) = -q1(0); L(1, 3) =  q1(1);
  L(2, 0) = -q1(1); L(2, 1) =  q1(0); L(2, 2) =  q1(3); L(2, 3) =  q1(2);
  L(3, 0) = -q1(0); L(3, 1) = -q1(1); L(3, 2) = -q1(2); L(3, 3) =  q1(3);

  Eigen::Vector4d q = L * q2;
  quaternionNormalize(q);
  return q;
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Section 3.2 "Kalman Filter Update" in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Vector4d smallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Vector4d q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.head<3>() = dq;
    q(3) = std::sqrt(1-dq_square_norm);
  } else {
    q.head<3>() = dq;
    q(3) = 1;
    q = q / std::sqrt(1+dq_square_norm);
  }

  return q;
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Section 3.2 "Kalman Filter Update" in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Quaterniond getSmallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Quaterniond q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.x() = dq(0);
    q.y() = dq(1);
    q.z() = dq(2);
    q.w() = std::sqrt(1-dq_square_norm);
  } else {
    q.x() = dq(0);
    q.y() = dq(1);
    q.z() = dq(2);
    q.w() = 1;
    q.normalize();
  }

  return q;
}

/*
 * @brief Convert a quaternion to the corresponding rotation matrix
 * @note Pay attention to the convention used. The function follows the
 *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for Quaternion Algebra", Equation (78).
 *
 *    The input quaternion should be in the form
 *      [q1, q2, q3, q4(scalar)]^T
 */
inline Eigen::Matrix3d quaternionToRotation(
    const Eigen::Vector4d& q) {
  // Hamilton
  const double& qw = q(3);
  const double& qx = q(0);
  const double& qy = q(1);
  const double& qz = q(2);
  Eigen::Matrix3d R;
  R(0, 0) = 1-2*(qy*qy+qz*qz);  R(0, 1) =   2*(qx*qy-qw*qz);  R(0, 2) =   2*(qx*qz+qw*qy);
  R(1, 0) =   2*(qx*qy+qw*qz);  R(1, 1) = 1-2*(qx*qx+qz*qz);  R(1, 2) =   2*(qy*qz-qw*qx);
  R(2, 0) =   2*(qx*qz-qw*qy);  R(2, 1) =   2*(qy*qz+qw*qx);  R(2, 2) = 1-2*(qx*qx+qy*qy);

  return R;
}

/*
 * @brief Convert a rotation matrix to a quaternion.
 * @note Pay attention to the convention used. The function follows the
 *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for Quaternion Algebra", Equation (78).
 *
 *    The input quaternion should be in the form
 *      [q1, q2, q3, q4(scalar)]^T
 */
inline Eigen::Vector4d rotationToQuaternion(
    const Eigen::Matrix3d& R) {
  Eigen::Vector4d score;
  score(0) = R(0, 0);
  score(1) = R(1, 1);
  score(2) = R(2, 2);
  score(3) = R.trace();

  int max_row = 0, max_col = 0;
  score.maxCoeff(&max_row, &max_col);

  Eigen::Vector4d q = Eigen::Vector4d::Zero();

  // Hamilton
  if (max_row == 0) {
    q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;
    q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));
    q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));
    q(3) = (R(2, 1)-R(1, 2)) / (4*q(0));
  } else if (max_row == 1) {
    q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;
    q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));
    q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));
    q(3) = (R(0, 2)-R(2, 0)) / (4*q(1));
  } else if (max_row == 2) {
    q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;
    q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));
    q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));
    q(3) = (R(1, 0)-R(0, 1)) / (4*q(2));
  } else {
    q(3) = std::sqrt(1+R.trace()) / 2.0;
    q(0) = (R(2, 1)-R(1, 2)) / (4*q(3));
    q(1) = (R(0, 2)-R(2, 0)) / (4*q(3));
    q(2) = (R(1, 0)-R(0, 1)) / (4*q(3));
  }

  if (q(3) < 0) q = -q;
  quaternionNormalize(q);
  return q;
}

inline Eigen::Matrix3d Hl_operator(const Eigen::Vector3d& gyro) {

  double gyro_norm = gyro.norm();

  Eigen::Matrix3d term1 = 0.5 * Eigen::Matrix3d::Identity();

  // handle the case when the input is exactly 0
  if (gyro_norm == 0)
  {
    return term1;
  }

  Eigen::Matrix3d term2 = ((gyro_norm - sin(gyro_norm)) / pow(gyro_norm, 3)) * skewSymmetric(gyro);
  Eigen::Matrix3d term3 = ((2*(cos(gyro_norm) - 1) + pow(gyro_norm, 2)) / (2*pow(gyro_norm, 4))) * (skewSymmetric(gyro) * skewSymmetric(gyro));

  Eigen::Matrix3d Hl = term1 + term2 + term3;

  return Hl;

}

inline Eigen::Matrix3d Jl_operator(const Eigen::Vector3d& gyro) {

  double gyro_norm = gyro.norm();

  Eigen::Matrix3d term1 = Eigen::Matrix3d::Identity();

  // handle the case when the input is exactly 0
  if (gyro_norm == 0)
  {
    return term1;
  }

  Eigen::Matrix3d term2 = ((1 - cos(gyro_norm)) / pow(gyro_norm, 2)) * skewSymmetric(gyro);
  Eigen::Matrix3d term3 = ((gyro_norm - sin(gyro_norm)) / pow(gyro_norm, 3)) * skewSymmetric(gyro) * skewSymmetric(gyro);

  Eigen::Matrix3d Jl = term1 + term2 + term3;

  return Jl;

}

inline Eigen::Matrix3d Sigma3_operator(const Eigen::Vector3d& gyro_hat, const Eigen::Matrix3d& Sigma0, const double& dt) {

  double gyro_norm = gyro_hat.norm();

  Eigen::Matrix3d term1 = (1.0 / 6.0) * Eigen::Matrix3d::Identity() * pow(dt, 3);

  // handle the case when the input is exactly 0
  if (gyro_norm == 0)
  {
    return term1;
  }

  Eigen::Matrix3d Theta_theta = -skewSymmetric(gyro_hat);

  Eigen::Matrix3d term2 = Theta_theta / pow(gyro_norm, 4);
  Eigen::Matrix3d term3 = Sigma0 - Eigen::Matrix3d::Identity() - Theta_theta * dt - 0.5 * Theta_theta * Theta_theta * pow(dt, 2) - (1.0 / 6.0) * Theta_theta * Theta_theta * Theta_theta * pow(dt, 3);

  Eigen::Matrix3d Sigma3 = term1 + term2 * term3;

  return Sigma3;

}

/*
 * @brief Inverse a Hamilton quaternion
 */
inline Eigen::Vector4d inverseQuaternion(
    const Eigen::Vector4d& quat) {

  Eigen::Vector4d q_inv;

  Eigen::Quaterniond quat_Q = Eigen::Quaterniond(quat);
  q_inv = quat_Q.inverse().coeffs();

  return q_inv;
}

inline bool nullspace_project_inplace_svd(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res)
{

  bool nullspace_trick_success_flag = true;

  if (H_f.rows() <= H_f.cols())
  {
    // insufficient residuals
    nullspace_trick_success_flag = false;
  }
  else
  {

    // Project the residual and Jacobians onto the nullspace of H_f.
    Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(H_f, Eigen::ComputeFullU | Eigen::ComputeThinV);
    Eigen::MatrixXd A = svd_helper.matrixU().rightCols(
            H_f.rows() - H_f.cols());

    H_x = (A.transpose() * H_x).eval();
    res = (A.transpose() * res).eval();

  }

  return nullspace_trick_success_flag;

}

// ref https://github.com/rpng/open_vins/blob/e169a77906941d662597bc83c8552e28b9a404f4/ov_msckf/src/update/UpdaterHelper.cpp
inline bool nullspace_project_inplace_qr(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {

    bool nullspace_trick_success_flag = true;

    if (H_f.rows()-H_f.cols() <= 0)
    {
      // insufficient residuals
      nullspace_trick_success_flag = false;
    }
    else
    {

      // ref https://docs.openvins.com/update-null.html
      Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(H_f.rows(), H_f.cols());
      qr.compute(H_f);
      Eigen::MatrixXd Q = qr.householderQ();
      Eigen::MatrixXd Q1 = Q.block(0,0,H_f.rows(),H_f.cols());
      Eigen::MatrixXd Q2 = Q.block(0,H_f.cols(),H_f.rows(),H_f.rows()-H_f.cols());

      H_x = (Q2.transpose() * H_x).eval();
      res = (Q2.transpose() * res).eval();

      // Sanity check
      assert(H_x.rows()==res.rows());

    }

    return nullspace_trick_success_flag;

}

inline Eigen::Vector4d unnormalize_bbox(const Eigen::Vector4d& bbox, const Eigen::Matrix3d& K) {
  Eigen::Vector4d nbbox;
  nbbox.head<2>() = K.block<2,2>(0,0) * bbox.head<2>() + K.topRightCorner<2,1>();
  nbbox.tail<2>() = K.block<2,2>(0,0) * bbox.tail<2>() + K.topRightCorner<2,1>();
  return nbbox;
}

inline Eigen::Vector4d normalize_bbox(const Eigen::Vector4d& bbox, const Eigen::Matrix3d& K) {

    // bbox format is
    // bbox.xmin, bbox.ymin, bbox.xmax, bbox.ymax

    Eigen::Vector4d nbbox;

    // for x
    nbbox(0) = (bbox(0) - K(0,2)) / K(0,0);
    nbbox(2) = (bbox(2) - K(0,2)) / K(0,0);

    // for y
    nbbox(1) = (bbox(1) - K(1,2)) / K(1,1);
    nbbox(3) = (bbox(3) - K(1,2)) / K(1,1);

    return nbbox;
}

// ref https://eigen.tuxfamily.org/bz/show_bug.cgi?id=448
template<typename Derived>
inline bool is_finite(const Eigen::MatrixBase<Derived>& x)
{
	return ( (x - x).array() == (x - x).array()).all();
}

template<typename Derived>
inline bool check_nan(const Eigen::MatrixBase<Derived>& x)
{
  // returns false if there is nan
  // since nan does not equal to itself
	return ((x.array() == x.array())).all();
}

} // end namespace orcvio

#endif // MATH_UTILS_HPP


// Copyright Paul A. 2007, 2010
// Copyright John Maddock 2006

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

// Simple example of computing probabilities and quantiles for
// a Bernoulli random variable representing the flipping of a coin.

// http://mathworld.wolfram.com/CoinTossing.html
// http://en.wikipedia.org/wiki/Bernoulli_trial
// Weisstein, Eric W. "Dice." From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/Dice.html
// http://en.wikipedia.org/wiki/Bernoulli_distribution
// http://mathworld.wolfram.com/BernoulliDistribution.html
//
// An idealized coin consists of a circular disk of zero thickness which,
// when thrown in the air and allowed to fall, will rest with either side face up
// ("heads" H or "tails" T) with equal probability. A coin is therefore a two-sided die.
// Despite slight differences between the sides and nonzero thickness of actual coins,
// the distribution of their tosses makes a good approximation to a p==1/2 Bernoulli distribution.

//[binomial_coinflip_example1

/*`An example of a [@http://en.wikipedia.org/wiki/Bernoulli_process Bernoulli process]
is coin flipping.
A variable in such a sequence may be called a Bernoulli variable.

This example shows using the Binomial distribution to predict the probability
of heads and tails when throwing a coin.

The number of correct answers (say heads),
X, is distributed as a binomial random variable
with binomial distribution parameters number of trials (flips) n = 10 and probability (success_fraction) of getting a head p = 0.5 (a 'fair' coin).

(Our coin is assumed fair, but we could easily change the success_fraction parameter p
from 0.5 to some other value to simulate an unfair coin,
say 0.6 for one with chewing gum on the tail,
so it is more likely to fall tails down and heads up).

First we need some includes and using statements to be able to use the binomial distribution, some std input and output, and get started:
*/

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

#include <iostream>
  using std::cout;  using std::endl;  using std::left;
#include <iomanip>
  using std::setw;

int main()
{
  cout << "Using Binomial distribution to predict how many heads and tails." << endl;
  try
  {
/*`
See note [link coinflip_eg_catch with the catch block]
about why a try and catch block is always a good idea.

First, construct a binomial distribution with parameters success_fraction
1/2, and how many flips.
*/
    const double success_fraction = 0.5; // = 50% = 1/2 for a 'fair' coin.
    int flips = 10;
    binomial flip(flips, success_fraction);

    cout.precision(4);
/*`
 Then some examples of using Binomial moments (and echoing the parameters).
*/
    cout << "From " << flips << " one can expect to get on average "
      << mean(flip) << " heads (or tails)." << endl;
    cout << "Mode is " << mode(flip) << endl;
    cout << "Standard deviation is " << standard_deviation(flip) << endl;
    cout << "So about 2/3 will lie within 1 standard deviation and get between "
      <<  ceil(mean(flip) - standard_deviation(flip))  << " and "
      << floor(mean(flip) + standard_deviation(flip)) << " correct." << endl;
    cout << "Skewness is " << skewness(flip) << endl;
    // Skewness of binomial distributions is only zero (symmetrical)
    // if success_fraction is exactly one half,
    // for example, when flipping 'fair' coins.
    cout << "Skewness if success_fraction is " << flip.success_fraction()
      << " is " << skewness(flip) << endl << endl; // Expect zero for a 'fair' coin.
/*`
Now we show a variety of predictions on the probability of heads:
*/
    cout << "For " << flip.trials() << " coin flips: " << endl;
    cout << "Probability of getting no heads is " << pdf(flip, 0) << endl;
    cout << "Probability of getting at least one head is " << 1. - pdf(flip, 0) << endl;
/*`
When we want to calculate the probability for a range or values we can sum the PDF's:
*/
    cout << "Probability of getting 0 or 1 heads is "
      << pdf(flip, 0) + pdf(flip, 1) << endl; // sum of exactly == probabilities
/*`
Or we can use the cdf.
*/
    cout << "Probability of getting 0 or 1 (<= 1) heads is " << cdf(flip, 1) << endl;
    cout << "Probability of getting 9 or 10 heads is " << pdf(flip, 9) + pdf(flip, 10) << endl;
/*`
Note that using
*/
    cout << "Probability of getting 9 or 10 heads is " << 1. - cdf(flip, 8) << endl;
/*`
is less accurate than using the complement
*/
    cout << "Probability of getting 9 or 10 heads is " << cdf(complement(flip, 8)) << endl;
/*`
Since the subtraction may involve
[@http://docs.sun.com/source/806-3568/ncg_goldberg.html cancellation error],
where as `cdf(complement(flip, 8))`
does not use such a subtraction internally, and so does not exhibit the problem.

To get the probability for a range of heads, we can either add the pdfs for each number of heads
*/
    cout << "Probability of between 4 and 6 heads (4 or 5 or 6) is "
      //  P(X == 4) + P(X == 5) + P(X == 6)
      << pdf(flip, 4) + pdf(flip, 5) + pdf(flip, 6) << endl;
/*`
But this is probably less efficient than using the cdf
*/
    cout << "Probability of between 4 and 6 heads (4 or 5 or 6) is "
      // P(X <= 6) - P(X <= 3) == P(X < 4)
      << cdf(flip, 6) - cdf(flip, 3) << endl;
/*`
Certainly for a bigger range like, 3 to 7
*/
    cout << "Probability of between 3 and 7 heads (3, 4, 5, 6 or 7) is "
      // P(X <= 7) - P(X <= 2) == P(X < 3)
      << cdf(flip, 7) - cdf(flip, 2) << endl;
    cout << endl;

/*`
Finally, print two tables of probability for the /exactly/ and /at least/ a number of heads.
*/
    // Print a table of probability for the exactly a number of heads.
    cout << "Probability of getting exactly (==) heads" << endl;
    for (int successes = 0; successes <= flips; successes++)
    { // Say success means getting a head (or equally success means getting a tail).
      double probability = pdf(flip, successes);
      cout << left << setw(2) << successes << "     " << setw(10)
        << probability << " or 1 in " << 1. / probability
        << ", or " << probability * 100. << "%" << endl;
    } // for i
    cout << endl;

    // Tabulate the probability of getting between zero heads and 0 upto 10 heads.
    cout << "Probability of getting upto (<=) heads" << endl;
    for (int successes = 0; successes <= flips; successes++)
    { // Say success means getting a head
      // (equally success could mean getting a tail).
      double probability = cdf(flip, successes); // P(X <= heads)
      cout << setw(2) << successes << "        " << setw(10) << left
        << probability << " or 1 in " << 1. / probability << ", or "
        << probability * 100. << "%"<< endl;
    } // for i
/*`
The last (0 to 10 heads) must, of course, be 100% probability.
*/
    double probability = 0.3;
    double q = quantile(flip, probability);
    std::cout << "Quantile (flip, " << probability << ") = " << q << std::endl; // Quantile (flip, 0.3) = 3
    probability = 0.6;
    q = quantile(flip, probability);
    std::cout << "Quantile (flip, " << probability << ") = " << q << std::endl; // Quantile (flip, 0.6) = 5
  }
  catch(const std::exception& e)
  {
    //
    /*`
    [#coinflip_eg_catch]
    It is always essential to include try & catch blocks because
    default policies are to throw exceptions on arguments that
    are out of domain or cause errors like numeric-overflow.

    Lacking try & catch blocks, the program will abort, whereas the
    message below from the thrown exception will give some helpful
    clues as to the cause of the problem.
    */
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
//] [binomial_coinflip_example1]
  return 0;
} // int main()

// Output:

//[binomial_coinflip_example_output
/*`

[pre
Using Binomial distribution to predict how many heads and tails.
From 10 one can expect to get on average 5 heads (or tails).
Mode is 5
Standard deviation is 1.581
So about 2/3 will lie within 1 standard deviation and get between 4 and 6 correct.
Skewness is 0
Skewness if success_fraction is 0.5 is 0

For 10 coin flips:
Probability of getting no heads is 0.0009766
Probability of getting at least one head is 0.999
Probability of getting 0 or 1 heads is 0.01074
Probability of getting 0 or 1 (<= 1) heads is 0.01074
Probability of getting 9 or 10 heads is 0.01074
Probability of getting 9 or 10 heads is 0.01074
Probability of getting 9 or 10 heads is 0.01074
Probability of between 4 and 6 heads (4 or 5 or 6) is 0.6562
Probability of between 4 and 6 heads (4 or 5 or 6) is 0.6563
Probability of between 3 and 7 heads (3, 4, 5, 6 or 7) is 0.8906

Probability of getting exactly (==) heads
0      0.0009766  or 1 in 1024, or 0.09766%
1      0.009766   or 1 in 102.4, or 0.9766%
2      0.04395    or 1 in 22.76, or 4.395%
3      0.1172     or 1 in 8.533, or 11.72%
4      0.2051     or 1 in 4.876, or 20.51%
5      0.2461     or 1 in 4.063, or 24.61%
6      0.2051     or 1 in 4.876, or 20.51%
7      0.1172     or 1 in 8.533, or 11.72%
8      0.04395    or 1 in 22.76, or 4.395%
9      0.009766   or 1 in 102.4, or 0.9766%
10     0.0009766  or 1 in 1024, or 0.09766%

Probability of getting upto (<=) heads
0         0.0009766  or 1 in 1024, or 0.09766%
1         0.01074    or 1 in 93.09, or 1.074%
2         0.05469    or 1 in 18.29, or 5.469%
3         0.1719     or 1 in 5.818, or 17.19%
4         0.377      or 1 in 2.653, or 37.7%
5         0.623      or 1 in 1.605, or 62.3%
6         0.8281     or 1 in 1.208, or 82.81%
7         0.9453     or 1 in 1.058, or 94.53%
8         0.9893     or 1 in 1.011, or 98.93%
9         0.999      or 1 in 1.001, or 99.9%
10        1          or 1 in 1, or 100%
]
*/
//][/binomial_coinflip_example_output]


#include <cmath>
#include<iostream>
#include <fstream>
#include <Eigen/Dense>
using namespace std;

class RBFN {

  private:
    double epseta = 0.5 ; //learning rate
    double seta = 0.0005; // seta
    int xn;  //2
    int k;   //5
    Eigen::MatrixXd b;
    Eigen::MatrixXd last_b ;
    Eigen::MatrixXd last_last_b ;

    Eigen::MatrixXd c;
    Eigen::MatrixXd last_c ;
    Eigen::MatrixXd last_last_c ;

    Eigen::MatrixXd w ;
    Eigen::MatrixXd last_w ;
    Eigen::MatrixXd last_last_w ;

    Eigen::MatrixXd dk;

    Eigen::MatrixXd  s_input;
    double Derivative_teacher_E;

    static double gaussian(double d, double b) {
        // exp( -(d^2)/2*s^2 )
        double tmp = - (d) / (2 * b*b);
        double ret = std::exp(tmp);
        return ret;
    }

  public:
    RBFN(int xn1, int k1) {
        xn = xn1;
        k = k1;
        init();

    }

    void init() {

        b = Eigen::MatrixXd::Random(k, 1);
        last_b = Eigen::MatrixXd::Random(k, 1);
        last_last_b = Eigen::MatrixXd::Random(k, 1);

        c = Eigen::MatrixXd::Random(k, xn);
        last_c = Eigen::MatrixXd::Random(k, xn);
        last_last_c = Eigen::MatrixXd::Random(k, xn);

        w = Eigen::MatrixXd::Random(k, 1);
        last_w = Eigen::MatrixXd::Random(k, 1);
        last_last_w = Eigen::MatrixXd::Random(k, 1);

        dk = Eigen::MatrixXd::Random(k, 1);
        cout << "w:    " << w <<endl;
        cout << "b:    " << b <<endl;
        cout << "c:    " << c <<endl;

    }
    void calc_distances() {
        int d;
        for (int i = 0; i < k; i++) {
            for (int j =0 ; j < xn; j++){
                d += pow( s_input(0,j)-c(i,j) , 2 );
            }
        dk(i,1) = d;
        }
    }

    void training(Eigen::MatrixXd s_input1,
                       double Derivative_teacher_E1) {
        s_input = s_input1;
        Derivative_teacher_E = Derivative_teacher_E1;

        //update the weight
        Eigen::MatrixXd w_next = w;
        Eigen::MatrixXd b_next = b;
        Eigen::MatrixXd c_next = c;
        for (int i = 0; i < k;i++){
            double delta_b =  Derivative_teacher_E * w(i,0) * gaussian(dk(i,0),b(i,0)) * (dk(i,0)) / pow(b(i,0),3);
            double delta_w =  Derivative_teacher_E * gaussian(dk(i,0),b(i,0));
            cout << "delta_w " << delta_w <<endl;
            w_next(i,0) = w(i,0) + epseta * delta_w + seta * (w(i,0) - last_w(i,0));
            b_next(i,0) = b(i,0) + epseta * delta_b + seta * (b(i,0) - last_b(i,0));
            for (int j =0; j < xn; j++){
                double delta_c =  Derivative_teacher_E * w(i,0) * (s_input(0,j) - c(i,j))/pow(b(i,0),2);
                c(i,j) = last_c(i,j) + epseta *delta_c + seta * (c(i,j) - last_c(i,j));
            }
        }

        last_w = w;
        w = w_next;
        last_b = b;
        b = b_next;
        last_c = c;
        c = c_next;

        //print the weitht
        cout << "w:    " << w <<endl;
        cout << "b:    " << b <<endl;
        cout << "c:    " << c <<endl;

    }

    double output() {
        double output;
        for (int i = 0; i < k;i++)
        {
           output = output + (w(i,0) * gaussian(dk(i,0),b(i,0)));
        }
        return output;
    }

    ~RBFN() {

    }

};


#include "MultivariateNormal.h"
#include <eigen3/Eigen/Dense>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <iostream>

MultivariateNormal::MultivariateNormal(const Eigen::MatrixXd& data,
                                       const Eigen::VectorXd& mu0,
                                       const Eigen::MatrixXd& S0,
                                       double k0,
                                       double v0)
    : LikelihoodFcn(data),
      phyper_(mu0, S0, k0, v0)
{
}

// Ref. https://www.cs.ubc.ca/~murphyk/Papers/bayesGauss.pdf page 21: Marginal likelihood
double MultivariateNormal::compute_marginal_log_likelihood(const std::set<std::size_t>& members) const
{
    NIWHyperParam hpost = get_posterior_hyperparameters(members);

    double marginal_ll = - ( static_cast<double>(members.size() * data_dimension()) / 2.0) * std::log( boost::math::constants::pi<double>() );
    marginal_ll += multivariate_log_gamma_ratio(hpost.v_/2.0, phyper_.v_/2.0, data_dimension() );
    marginal_ll += (phyper_.v_ / 2.0) * std::log( phyper_.S_.determinant() ) - (hpost.v_ / 2.0) * std::log( hpost.S_.determinant() );
    marginal_ll += (static_cast<double>( data_dimension() ) / 2.0) * ( std::log(phyper_.k_) - std::log(hpost.k_)  );

    return marginal_ll;
}

NIWHyperParam MultivariateNormal::get_posterior_hyperparameters(const std::set<std::size_t>& members) const
{
    // Murphy: Machine learning - a probabilistic perspective
    // Sect. 4.6.3.3
    const double num_data = static_cast<double>( members.size() );
    NIWHyperParam hpost(phyper_);
    hpost.mu_ = ( phyper_.k_ / ( phyper_.k_ + num_data ) ) * phyper_.mu_ + (num_data / ( phyper_.k_ + num_data ) ) * sample_mean(members);
    hpost.k_ += num_data;
    hpost.v_ += num_data;
    hpost.S_ += sample_uncentered_sum_of_squares_matrix(members) + phyper_.k_ * ( phyper_.mu_ * phyper_.mu_.transpose() ) - hpost.k_ * ( hpost.mu_ * hpost.mu_.transpose() );

    return hpost;
}

double MultivariateNormal::multivariate_log_gamma_ratio(double a, double b, std::size_t d)
{
    double y = 0.0;
    for ( std::size_t i = 1; i <= d; ++i)
    {
        const double d = (1.0 - static_cast<double>(i)) / 2.0;
        try
        {
            //y += std::log( boost::math::tgamma_ratio(a+d, b+d) );
            y += std::lgamma(a+d) - std::lgamma(b+d);
        }
        catch (...)
        {
            std::cout << "Failure with a+d = " << a+d << " and b+d = " << b + d << " (d = " << d << ")\n";
            throw;
        }
    }

    return y;
}


//
//  main.cpp
//  5 - Normal Distribution
//
//  Created by Zhehao Li on 2020/4/16.
//  Copyright © 2020 Zhehao Li. All rights reserved.
//

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions.hpp>             // For non-member functions of distributions

#include <vector>
#include <iostream>
using namespace std;

int main(int argc, const char * argv[]) {
    // Don't forget to tell compiler which namespace
    using namespace boost::math;

    // 1. Create normal distribution object
    normal_distribution<> myNormal(1.0, 10.0);          // Default type is 'double'
    cout << "Mean: " << mean(myNormal) << ", standard deviation: " << standard_deviation(myNormal) << endl;

    // 1.1 Distributional properties
    double x = 10.25;

    cout << "pdf: " << pdf(myNormal, x) << endl;
    cout << "cdf: " << cdf(myNormal, x) << endl;

    // 1.2 Choose another data type and now a N(0,1) variate
    normal_distribution<float> myNormal2;
    cout << "Mean: " << mean(myNormal2) << ", standard deviation: " << standard_deviation(myNormal2) << endl;

    cout << "pdf: " << pdf(myNormal2, x) << endl;
    cout << "cdf: " << cdf(myNormal2, x) << endl;

    // 1.3 Choose precision
    cout.precision(10); // Number of values behind the comma

    // 1.4 Other properties
    cout << "\n*** normal distribution ***: \n";
    cout << "mean: " << mean(myNormal) << endl;
    cout << "variance: " << variance(myNormal) << endl;
    cout << "median: " << median(myNormal) << endl;
    cout << "mode: " << mode(myNormal) << endl;
    cout << "kurtosis excess: " << kurtosis_excess(myNormal) << endl;
    cout << "kurtosis: " << kurtosis(myNormal) << endl;
    cout << "characteristic function: " << chf(myNormal, x) << endl;
    cout << "hazard: " << hazard(myNormal, x) << endl;

    // 2. Gamma distribution
    double alpha = 3.0;                                 // Shape parameter, k
    double beta = 0.5;                                  // Scale parameter, theta
    gamma_distribution<double> myGamma(alpha, beta);

    double val = 13.0;
    cout << endl <<  "pdf: " << pdf(myGamma, val) << endl;
    cout << "cdf: " << cdf(myGamma, val) << endl;

    vector<double> pdfList;
    vector<double> cdfList;

    double start = 0.0;
    double end = 10.0;
    long N = 30;        // Number of subdivisions

    val = 0.0;
    double h = (end - start) / double(N);

    for (long j = 1; j <= N; ++j){
        pdfList.push_back(pdf(myGamma, val));
        cdfList.push_back(cdf(myGamma, val));

        val += h;
    }

    for (long j = 0; j < pdfList.size(); ++j){
        cout << pdfList[j] << ", ";
    }

    cout << "***" << endl;

    for (long j = 0; j < cdfList.size(); ++j){
        cout << cdfList[j] << ", ";
    }

    return 0;
}

Level_8/V. C++ Boost/5 - Normal Distribution/5 - Normal Distribution/main.cpp


/**
 * @file linquadlagrfe.cc
 * @brief Creates convergence plots for experiment 3.2.3.7 and 3.2.3.8
 * @author Tobias Rohner
 * @date April 2020
 * @copyright MIT License
 */

#define _USE_MATH_DEFINES

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/quad/quad.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <cmath>
#include <cstdlib>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>

namespace po = boost::program_options;

/**
 * @brief Solves the Poisson problem on a given mesh
 * @param mesh The mesh on which to solve the PDE
 * @param fe_space The Finite Element Space to use
 * @returns The dof vector corresponding to the solution of the PDE
 *
 * The Dirichlet boundary conditions are set to zero and the load is given by
 * \f[
        f(x) = 2\pi^2\sin(\pi x_1)\sin\pi x_2)
   \f]
 */
Eigen::VectorXd solvePoisson(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>>
        &fe_space) {
  // Define the load function of the manufactured solution
  const auto load = [](const Eigen::Vector2d &x) -> double {
    return 2 * M_PI * M_PI * std::sin(M_PI * x[0]) * std::sin(M_PI * x[1]);
  };
  const lf::mesh::utils::MeshFunctionGlobal mf_load(load);

  // Intialize the matrix and vector providers
  const lf::mesh::utils::MeshFunctionConstant<double> mf_alpha(1);
  const lf::mesh::utils::MeshFunctionConstant<double> mf_gamma(0);
  lf::uscalfe::ReactionDiffusionElementMatrixProvider element_matrix_provider(
      fe_space, mf_alpha, mf_gamma);
  lf::uscalfe::ScalarLoadElementVectorProvider element_vector_provider(
      fe_space, mf_load,
      {{lf::base::RefEl::kTria(),
        lf::quad::make_QuadRule(lf::base::RefEl::kTria(), 6)},
       {lf::base::RefEl::kQuad(),
        lf::quad::make_QuadRule(lf::base::RefEl::kQuad(), 6)}});

  // Assemble the system matrix and right hand side
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  lf::assemble::COOMatrix<double> A_COO(dofh.NumDofs(), dofh.NumDofs());
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  std::cout << "\t\t> Assembling System Matrix" << std::endl;
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, element_matrix_provider,
                                      A_COO);
  std::cout << "\t\t> Assembling right Hand Side" << std::endl;
  lf::assemble::AssembleVectorLocally(0, dofh, element_vector_provider, rhs);

  // Enforce zero dirichlet boundary conditions
  std::cout << "\t\t> Enforcing Boundary Conditions" << std::endl;
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  const auto selector = [&](unsigned int idx) -> std::pair<bool, double> {
    return {boundary(dofh.Entity(idx)), 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A_COO, rhs);

  // Solve the LSE using the cholesky decomposition
  std::cout << "\t\t> Solving LSE" << std::endl;
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();
  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
  Eigen::VectorXd solution = solver.solve(rhs);

  // Return the resulting dof vector
  return solution;
}

int main(int argc, char *argv[]) {
  const int num_meshes = 7;

  po::options_description desc("allowed options");
  desc.add_options()("output,o", po::value<std::string>(),
                     "Name of the output file");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("output") == 0) {
    std::cout << desc << std::endl;
    exit(1);
  }
  const std::string output_file = vm["output"].as<std::string>();

  // The analytic solution
  const auto u = [](const Eigen::VectorXd &x) -> double {
    return std::sin(M_PI * x[0]) * std::sin(M_PI * x[1]);
  };
  const lf::mesh::utils::MeshFunctionGlobal mf_u(u);
  // The gradient of the analytic solution
  const auto u_grad = [](const Eigen::Vector2d &x) -> Eigen::Vector2d {
    Eigen::Vector2d grad;
    grad[0] = M_PI * std::cos(M_PI * x[0]) * std::sin(M_PI * x[1]);
    grad[1] = M_PI * std::sin(M_PI * x[0]) * std::cos(M_PI * x[1]);
    return grad;
  };
  const lf::mesh::utils::MeshFunctionGlobal mf_u_grad(u_grad);

  const std::filesystem::path here = __FILE__;
  const std::filesystem::path mesh_folder = here.parent_path() / "meshes";
  Eigen::MatrixXd results(num_meshes, 7);
  for (int mesh_idx = 0; mesh_idx < num_meshes; ++mesh_idx) {
    std::cout << "> Mesh Nr. " << mesh_idx << std::endl;

    // Load the mesh
    std::cout << "\t> Loading Mesh" << std::endl;
    const std::string mesh_name =
        "unitsquare" + std::to_string(mesh_idx) + ".msh";
    const std::filesystem::path mesh_file = mesh_folder / mesh_name;
    auto factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    const lf::io::GmshReader reader(std::move(factory), mesh_file.string());
    const auto mesh = reader.mesh();

    // Solve the problem with linear finite elements
    std::cout << "\t> Linear Lagrangian FE";
    const auto fe_space_o1 =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
    std::cout << " (" << fe_space_o1->LocGlobMap().NumDofs() << " DOFs)"
              << std::endl;
    const Eigen::VectorXd solution_o1 = solvePoisson(mesh, fe_space_o1);
    const lf::fe::MeshFunctionFE<double, double> mf_o1(fe_space_o1,
                                                       solution_o1);
    const lf::fe::MeshFunctionGradFE<double, double> mf_grad_o1(fe_space_o1,
                                                                solution_o1);

    // Solve the problem with quadratic finite elements
    std::cout << "\t> Quadratic Lagrangian FE";
    const auto fe_space_o2 =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO2<double>>(mesh);
    std::cout << " (" << fe_space_o2->LocGlobMap().NumDofs() << " DOFs)"
              << std::endl;
    const Eigen::VectorXd solution_o2 = solvePoisson(mesh, fe_space_o2);
    const lf::fe::MeshFunctionFE<double, double> mf_o2(fe_space_o2,
                                                       solution_o2);
    const lf::fe::MeshFunctionGradFE<double, double> mf_grad_o2(fe_space_o2,
                                                                solution_o2);

    // Compute the H1 and L2 errors
    std::cout << "\t> Computing Error Norms" << std::endl;
    const auto quadrule_provider = [](const lf::mesh::Entity &entity) {
      return lf::quad::make_QuadRule(entity.RefEl(), 6);
    };
    const double H1_err_o1 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_grad_o1 - mf_u_grad),
        quadrule_provider));
    const double H1_err_o2 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_grad_o2 - mf_u_grad),
        quadrule_provider));
    const double L2_err_o1 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_o1 - mf_u), quadrule_provider));
    const double L2_err_o2 = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh, lf::mesh::utils::squaredNorm(mf_o2 - mf_u), quadrule_provider));

    // Store the mesh width, the number of DOFs and the errors in the results
    // matrix
    results(mesh_idx, 0) = std::sqrt(1. / mesh->NumEntities(0));
    results(mesh_idx, 1) = fe_space_o1->LocGlobMap().NumDofs();
    results(mesh_idx, 2) = fe_space_o2->LocGlobMap().NumDofs();
    results(mesh_idx, 3) = H1_err_o1;
    results(mesh_idx, 4) = H1_err_o2;
    results(mesh_idx, 5) = L2_err_o1;
    results(mesh_idx, 6) = L2_err_o2;
  }

  // Output the resulting errors to a file
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream file;
  file.open(output_file);
  file << results.format(CSVFormat);
  file.close();

  return 0;
}


#include<iostream>
#include <math.h>
#include<fstream>
#include<vector>
#include <Eigen/Dense>
#include<string>

#define	TOTAL_ITERATIONS	50000

using Eigen::MatrixXd;
using namespace std;

//helper function declaration:
MatrixXd* read_matrix(string filename);
void write_matrix(string filename, MatrixXd m);
void write_vec(string filename, vector<double> m);

//      MAIN PROGRAM
int main(){
    string casename = "A";
    // import the data required from A,B and C files

    MatrixXd* data = read_matrix("Data_"+ casename + "_lab_2.csv");

    double	an,an1,fan,fpan, xi, yi;
    int	i,j;

    an = 0.1;	// initial guess of a
    vector<double> v;
    v.push_back(an);
    for (j=0; j<TOTAL_ITERATIONS; j++){
        fan=fpan=0.0;
        for (i=0; i<(*data).rows(); i++){
            // cout<<i<<endl;
            xi = (*data)(i,0);
            yi = (*data)(i,1);

            fan+= (yi - log(an * xi)) * 1/an;
            fpan+= (- 1/an) * 1/an - (yi - log(an * xi)) * 1/(an*an);
        }
        an1=an-fan/fpan;

        printf("%lf  %lf\n",an,an1);
        if (fabs(an-an1) < 0.00000001)
            break;
        v.push_back(fabs(an-an1));
        an=an1;

    }
    printf("%d iterations\n",j);
    // exporting data fit:
    MatrixXd XiYi(600,2);
    for(int i=0; i<600; i++){
        XiYi(i,0) = (i+1)/10;                 // store the x value
        XiYi(i,1) = log(an * ((i+1)/10));       // store the y value
    }

    if (casename=="A"){
    write_vec("iterations_A.txt", v);
    write_matrix("A_fit.txt", XiYi);}
    else if (casename=="B"){
    write_vec("iterations_B.txt", v);
    write_matrix("B_fit.txt", XiYi);
    }
    else if (casename=="C"){
    write_vec("iterations_C.txt", v);
    write_matrix("C_fit.txt", XiYi);
    }
    return 0;
}

MatrixXd* read_matrix(string filename){
    ifstream myfile;
    myfile.open(filename);
    if (!myfile.is_open()) throw runtime_error("Could not open file");
    string row_item;
    vector<vector<double> > rows;
    string line;
    while(myfile.good()){
        vector<double> row;
        getline(myfile, line);
        // cout<<line<<endl;
        stringstream s(line);
        while (getline(s,row_item,' ')){
            // cout<<row_item<<" ";
            row.push_back(stod(row_item));
        }
        rows.push_back(row);
    }
    myfile.close();
    int nrows,ncols;
    nrows = rows.end() - rows.begin();
    ncols = rows[0].end() - rows[0].begin();
    MatrixXd* m = new MatrixXd(nrows,ncols);
    for(int i=0; i<nrows; i++){
        for (int j=0; j<ncols; j++){
            (*m)(i,j) = rows[i][j];
            // cout<<rows[i][j]<<" ";
        }
    }
    // cout<<"The following is a matrix of "<<nrows<<" rows and "<<ncols<<" coloumns: "<<endl<<*m<<endl;
    return m;
}

void write_matrix(string filename, MatrixXd m){
    ofstream(myfile);
    myfile.open(filename);
    for(int i=0; i<m.rows(); i++){
        for(int j=0; j<m.cols(); j++){
            myfile << m(i,j) <<",";
        }
        myfile << endl;
    }
}

void write_vec(string filename, vector<double> m){
    ofstream(myfile);
    myfile.open(filename);
    for(int i=0; i<m.end()-m.begin(); i++){
            myfile << m[i] <<endl;
        }
}


# pragma once

#include <vector>
#include <map>
#include <memory>

#include <math.h>
#include <Eigen/QR>
#include <Eigen/Dense>

using std::vector;

namespace GRANSAC
{

class Regression2D
{
public:

	static void calculateLLS_QR(const std::vector<float> &xv, const std::vector<float> &yv, std::vector<float> &coeff, int order)
    {
		Eigen::MatrixXf A(xv.size(), order+1);
		Eigen::VectorXf yv_mapped = Eigen::VectorXf::Map(&yv.front(), yv.size());
		Eigen::VectorXf result;

		assert(xv.size() == yv.size());
		assert(xv.size() >= order+1);

		// create matrix
		for (size_t i = 0; i < xv.size(); i++)
		for (size_t j = 0; j < order+1; j++)
			A(i, j) = pow(xv.at(i), j);

		// solve for linear least squares fit
		result = A.householderQr().solve(yv_mapped);

		coeff.resize(order+1);
		for (size_t i = 0; i < order+1; i++)
			coeff[i] = result[i];
	}

    static void calculateLLS(const std::vector<float> &xv, const std::vector<float> &yv, std::vector<float> &coeff, int order)
    {
        // faster but with larger condition number compared with QR method
		Eigen::MatrixXf A(xv.size(), order+1);
		Eigen::VectorXf yv_mapped = Eigen::VectorXf::Map(&yv.front(), yv.size());
		Eigen::VectorXf result;

		assert(xv.size() == yv.size());
		assert(xv.size() >= order+1);

		// create matrix
		for (size_t i = 0; i < xv.size(); i++)
		for (size_t j = 0; j < order+1; j++)
			A(i, j) = pow(xv.at(i), j);

		// solve for linear least squares fit
		result = (A.transpose() * A).ldlt().solve(A.transpose() * yv_mapped);

		coeff.resize(order+1);
		for (size_t i = 0; i < order+1; i++)
			coeff[i] = result[i];
	}

	static void calculateLLS_L2(const std::vector<float> &xv, const std::vector<float> &yv, std::vector<float> &coeff, int order, float r_term=0.0f)
    {
        // faster but with larger condition number compared with QR method
		Eigen::MatrixXf A(xv.size(), order+1);
		Eigen::VectorXf yv_mapped = Eigen::VectorXf::Map(&yv.front(), yv.size());
		Eigen::VectorXf result;

		assert(xv.size() == yv.size());
		assert(xv.size() >= order+1);

		// create matrix
		for (size_t i = 0; i < xv.size(); i++)
		for (size_t j = 0; j < order+1; j++)
			A(i, j) = pow(xv.at(i), j);

		// solve for linear least squares fit
		result = (A.transpose() * A + r_term * Eigen::MatrixXf::Identity(A.cols(), A.cols())).ldlt().solve(A.transpose() * yv_mapped);

		coeff.resize(order+1);
		for (size_t i = 0; i < order+1; i++)
			coeff[i] = result[i];
	}

};

}


#include <iostream>
#include <vector>
// #include <unordered_map>
// #include <boost/functional/hash.hpp>
#include <map>
#include <algorithm>

using namespace std;

typedef std::pair<int, int> int_pair;

class Solution {
public:
    int maxPoints(vector<vector<int>>& points) {
        int n = points.size();
        int result = 0;
        // unordered_map<int_pair, int, boost::hash<int_pair> > gradients;
        map<int_pair, int> gradients;
        for (int i = 0; i < n - result; ++i) {
            gradients.clear();
            int over = 1, vertical = 0, cur_max = 0;
            for (int j = i+1; j < n; ++j) {
                if (points[i][0] == points[j][0]) {
                    if (points[i][1] == points[j][1]) over++;
                    else if (++vertical > cur_max) cur_max = vertical;
                    continue;
                }
                int dy = points[j][1] - points[i][1];
                int dx = points[j][0] - points[i][0];
                int div = __gcd(dy, dx);
                int_pair i_pair = {dy / div, dx / div};
                if (++gradients[i_pair] > cur_max) {
                    cur_max = gradients[i_pair];
                }
            }
            result = max(result, cur_max + over);
        }
        return result;
    }
};

int main(int argc, char const *argv[])
{
    vector<vector<int>> points = {{84,250},{0,0},{1,0},{0,-70},{0,-70},{1,-1},{21,10},{42,90},{-42,-230}};
    vector<vector<int>> points2 = {{0,0},{94911151,94911150},{94911152,94911151}};
    Solution s;
    cout<<s.maxPoints(points2)<<endl;
    return 0;
}

leetcode.com/0149 Max Points on a Line/main.cpp


#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include "sophus/se3.hpp"

typedef std::vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

std::string estimated_file_path = "estimated.txt";
std::string groundtruth_file_path = "groundtruth.txt";

TrajectoryType ReadTrajectory(std::string &file_path){
    std::fstream fin(file_path);
    TrajectoryType trajectory;
    double time, qx, qy, qz, qw, tx, ty, tz;

    if (!fin){
        std::cerr << "cannot open file " << file_path << "!" << std::endl;
    }

    while(!fin.eof()){
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;

        Eigen::Quaterniond q(qx, qy, qz, qw);
        Eigen::Vector3d t(tx, ty, tz);
        Sophus::SE3d T(q, t);

        trajectory.push_back(T);
    }

    return trajectory;
}

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

    TrajectoryType estimated_trajectory = ReadTrajectory(estimated_file_path);
    TrajectoryType groundtruth_trajectory = ReadTrajectory(groundtruth_file_path);

    if (estimated_trajectory.size() != groundtruth_trajectory.size()){
        std::cerr << "the length of estimate_trajectory != length of groundtruth trajectory!" << std::endl;
    }

    double total_error = 0, error;

    for(size_t i = 0; i < estimated_trajectory.size(); i++){
        Sophus::SE3d T_estimated = estimated_trajectory[i];
        Sophus::SE3d T_groundtruth = groundtruth_trajectory[i];

        error = (T_groundtruth.inverse() * T_estimated).log().norm();
        total_error += error * error;
    }

    double rmse = sqrt(total_error / estimated_trajectory.size());

    std::cout << "RMSE = " << rmse << std::endl;

    return 0;
}


/*
 * Copyright (C) 2019 by AutoSense Organization. All rights reserved.
 * Gary Chan <chenshj35@mail2.sysu.edu.cn>
 */

#ifndef COMMON_INCLUDE_COMMON_GEOMETRY_HPP_
#define COMMON_INCLUDE_COMMON_GEOMETRY_HPP_

#include <pcl/point_cloud.h>
#include <Eigen/Core>

#include "common/types/object.hpp"  // ObjectPtr

namespace autosense {
namespace common {
namespace geometry {
/**
 * @brief compute velocity's angle change between "v1" and "v2"
 * @note
 *  v1·v2 = |v1|*|v2|cos(theta)
 *  v1xv2 = |v1|*|v2|sin(theta)
 * @param v1
 * @param v2
 * @return
 */
template <typename VectorT>
static double computeTheta2dXyBetweenVectors(const VectorT& v1,
                                             const VectorT& v2) {
    double v1_len = sqrt((v1.head(2).cwiseProduct(v1.head(2))).sum());
    double v2_len = sqrt((v2.head(2).cwiseProduct(v2.head(2))).sum());

    // cos(theta) = (v1·v2)/(|v1|*|v2|) = [v1(0)*v2(0)+v1(1)*v2(1)]/(|v1|*|v2|)
    double cos_theta =
        (v1.head(2).cwiseProduct(v2.head(2))).sum() / (v1_len * v2_len);
    // limit theta to [0, PI] or [-PI, 0]
    if (cos_theta > 1) {
        cos_theta = 1;
    }
    if (cos_theta < -1) {
        cos_theta = -1;
    }

    // sin(theta) = (v1xv2)/(|v1|*|v2|) = [v1(0)*v2(1)-v1(1)*v2(0)]/(|v1|*|v2|)
    double sin_theta = (v1(0) * v2(1) - v1(1) * v2(0)) / (v1_len * v2_len);
    // return [0, PI]
    double theta = acos(cos_theta);
    if (sin_theta < 0) {
        theta = -theta;
    }

    return theta;
}

/**
 * @breif compute velocity's angle cos value between "v1" and "v2"
 * @param v1
 * @param v2
 * @return
 */
static double computeCosTheta2dXyBetweenVectors(const Eigen::Vector3f& v1,
                                                const Eigen::Vector3f& v2) {
    double v1_len = sqrt((v1.head(2).cwiseProduct(v1.head(2))).sum());
    double v2_len = sqrt((v2.head(2).cwiseProduct(v2.head(2))).sum());
    double cos_theta =
        (v1.head(2).cwiseProduct(v2.head(2))).sum() / (v1_len * v2_len);
    return cos_theta;
}

/**
 * @breif compute 3D point's Sphere/Cylinder distance and corresponding norm
 * @tparam PointT
 * @param point
 * @return
 */
template <typename PointT>
static float calcSphereDistNorm(PointT point) {
    return pow(point.x, 2.0) + pow(point.y, 2.0) + pow(point.z, 2.0);
}

template <typename PointT>
static float calcSphereDist(PointT point) {
    return sqrt(calcSphereDistNorm<PointT>(point));
}

template <typename PointT>
static float calcCylinderDistNorm(PointT point) {
    return pow(point.x, 2.0) + pow(point.y, 2.0);
}

template <typename PointT>
static float calcCylinderDist(PointT point) {
    return sqrt(calcCylinderDistNorm<PointT>(point));
}

// 计算点云重心
template <typename PointT>
static Eigen::Vector3d getCloudBarycenter(
    typename pcl::PointCloud<PointT>::ConstPtr cloud) {
    int num_points = cloud->points.size();
    Eigen::Vector3d barycenter(0, 0, 0);

    for (int i = 0; i < num_points; i++) {
        const PointT& pt = cloud->points[i];
        barycenter[0] += pt.x;
        barycenter[1] += pt.y;
        barycenter[2] += pt.z;
    }

    if (num_points > 0) {
        barycenter[0] /= num_points;
        barycenter[1] /= num_points;
        barycenter[2] /= num_points;
    }
    return barycenter;
}

/**
 * @brief calculate yaw given direction vector, suppose coordinate vector: (1.0,
 * 0.0, 0.0)
 * @tparam VectorT
 * @param dir
 * @return yaw in rad
 */
template <typename VectorT>
static double calcYaw4DirectionVector(const VectorT& dir) {
    const Eigen::Vector3d coord_dir(1.0, 0.0, 0.0);
    return computeTheta2dXyBetweenVectors<Eigen::Vector3d>(coord_dir, dir);
}

}  // namespace geometry
}  // namespace common
}  // namespace autosense

#endif  // COMMON_INCLUDE_COMMON_GEOMETRY_HPP_


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

/**
 * @brief Calcola la direzione normale di ogni triangolo nella mesh in input.
 *
 * Data una mesh di triangoli M = (V, F), dove V e' un array di posizioni 3D
 * (i vertici) e F e' un array di triple di indici ai vertici (ogni tripla
 * rappresenta una faccia triangolare), questa funzione calcola la direzione
 * normale di ogni triangolo.
 *
 * La direzione normale di un triangolo e' la direzione normale del piano su
 * cui giace. Questa viene calcolata come il prodotto vettoriale tra i vettori
 * che rappresentano due lati del triangolo centrati sul vertice a comune,
 * presi in senso antiorario, e normalizzato (cioe' reso unitario: lunghezza 1):
 *
 *             C
 *             *
 *           /  \                         (B - A) ^ (C - A)         ^
 *        /      \                  n = ---------------------       |
 *     /          \                     ||(B - A) ^ (C - A)||
 *  A *------------* B
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @return MatrixXd La matrice restituita ha una riga per ogni faccia triangolare
 *                  (N.rows() == F.rows()) e 3 colonne. Ogni riga corrisponde
 *                  alla direzione normale del triangolo corrispondente, avente
 *                  le 3 coordinate x,y,z.
 */
MatrixXd perFaceNormals(MatrixXd const &V, MatrixXi const &F)
{
    MatrixXd N(F.rows(), 3);

    // per leggere/scrivere un elemento di una matrice X: X(i,j)
    // per leggere/scrivere una riga di una matrice X: X.row(i) (un vettore orizzontale)
    // il vettore da un punto p0 a un altro punto p1: Vector3d v = p1 - p0;
    // il prodotto vettoriale tra 2 vettori: Vector3d c = v1.cross(v2);
    // normalizzare un vettore, in-place: c.normalize();
    // normalizzare un vettore, in una nuova variabile: Vector3d cn = c.normalized();

    for (int f = 0; f < F.rows(); ++f)
    {
        Vector3d A = V.row(F(f, 0));
        Vector3d B = V.row(F(f, 1));
        Vector3d C = V.row(F(f, 2));
        Vector3d e0 = B - A;
        Vector3d e1 = C - A;
        N.row(f) = e0.cross(e1).normalized();
    }

    return N;
}


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2021.                        //
//  Distributed under the Boost Software License,                //
//  Version 1.0. (See accompanying file LICENSE_1_0.txt          //
//  or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////

#include <cmath>

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/bindings/decwide_t.hpp>

#include <math/wide_decimal/decwide_t_examples.h>

namespace math { namespace wide_decimal {

namespace detail {

template<typename FloatingPointType>
FloatingPointType sin_series(const FloatingPointType& x)
{
        FloatingPointType term        = x;
  const FloatingPointType x2          = x * x;
        FloatingPointType sum         = x;
        bool              term_is_neg = true;
  const FloatingPointType tol         = std::numeric_limits<FloatingPointType>::epsilon() * x;

  for(std::uint32_t k = 3U; k < UINT32_C(0xFFFF); k += 2U)
  {
    term *= x2;
    term /= std::uint32_t(k * std::uint32_t(k - 1U));

    if(term < tol)
    {
      break;
    }

    term_is_neg ? sum -= term : sum += term;

    term_is_neg = (!term_is_neg);
  }

  return sum;
}

template<typename FloatingPointType>
FloatingPointType cos_series(const FloatingPointType& x)
{
  const FloatingPointType x2          = x * x;
        FloatingPointType term        = x2 / 2U;
        FloatingPointType sum         = term;
        bool              term_is_neg = true;
  const FloatingPointType tol         = std::numeric_limits<FloatingPointType>::epsilon() * x;

  for(std::uint32_t k = 4U; k < UINT32_C(0xFFFF); k += 2U)
  {
    term *= x2;
    term /= std::uint32_t(k * std::uint32_t(k - 1U));

    if(term < tol)
    {
      break;
    }

    term_is_neg ? sum -= term : sum += term;

    term_is_neg = (!term_is_neg);
  }

  return 1U - sum;
}

}

template<typename FloatingPointType>
FloatingPointType sin(const FloatingPointType& x)
{
  using floating_point_type = FloatingPointType;

  floating_point_type s;

  if(x < 0)
  {
    s = -sin(-x);
  }
  else if(x > 0)
  {
    // Perform argument reduction and subsequent scaling of the result.

    // Given x = k * (pi/2) + r, compute n = (k % 4).

    // | n |  sin(x) |  cos(x) |  sin(x)/cos(x) |
    // |----------------------------------------|
    // | 0 |  sin(r) |  cos(r) |  sin(r)/cos(r) |
    // | 1 |  cos(r) | -sin(r) | -cos(r)/sin(r) |
    // | 2 | -sin(r) | -cos(r) |  sin(r)/cos(r) |
    // | 3 | -cos(r) |  sin(r) | -cos(r)/sin(r) |

    const std::uint32_t      k = (std::uint32_t) (x / boost::math::constants::half_pi<floating_point_type>());
    const std::uint_fast32_t n = (std::uint_fast32_t) (k % 4U);

    floating_point_type r = x - (boost::math::constants::half_pi<floating_point_type>() * k);

    const bool is_neg =  (n > 1U);
    const bool is_cos = ((n == 1U) || (n == 3U));

    std::uint_fast32_t n_angle_identity = 0U;

    static const floating_point_type one_tenth = floating_point_type(1U) / 10U;

    // Reduce the argument with factors of three until it is less than 1/10.
    while(r > one_tenth)
    {
      r /= 3U;

      ++n_angle_identity;
    }

    s = (is_cos ? detail::cos_series(r) : detail::sin_series(r));

    // Rescale the result with the triple angle identity for sine.
    for(std::uint_fast32_t t = 0U; t < n_angle_identity; ++t)
    {
      s = (s * 3U) - (((s * s) * s) * 4U);
    }

    s = fabs(s);

    if(is_neg)
    {
      s = -s;
    }
  }
  else
  {
    s = 0;
  }

  return s;
}

template<typename FloatingPointType>
FloatingPointType cos(const FloatingPointType& x)
{
  using floating_point_type = FloatingPointType;

  floating_point_type c;

  if(x < 0)
  {
    c = cos(-x);
  }
  else if(x > 0)
  {
    // Perform argument reduction and subsequent scaling of the result.

    // Given x = k * (pi/2) + r, compute n = (k % 4).

    // | n |  sin(x) |  cos(x) |  sin(x)/cos(x) |
    // |----------------------------------------|
    // | 0 |  sin(r) |  cos(r) |  sin(r)/cos(r) |
    // | 1 |  cos(r) | -sin(r) | -cos(r)/sin(r) |
    // | 2 | -sin(r) | -cos(r) |  sin(r)/cos(r) |
    // | 3 | -cos(r) |  sin(r) | -cos(r)/sin(r) |

    const std::uint32_t      k = (std::uint32_t) (x / boost::math::constants::half_pi<floating_point_type>());
    const std::uint_fast32_t n = (std::uint_fast32_t) (k % 4U);

    floating_point_type r = x - (boost::math::constants::half_pi<floating_point_type>() * k);

    const bool is_neg = ((n == 1U) || (n == 2U));
    const bool is_sin = ((n == 1U) || (n == 3U));

    std::uint_fast32_t n_angle_identity = 0U;

    static const floating_point_type one_tenth = floating_point_type(1U) / 10U;

    // Reduce the argument with factors of three until it is less than 1/10.
    while(r > one_tenth)
    {
      r /= 3U;

      ++n_angle_identity;
    }

    c = (is_sin ? detail::sin_series(r) : detail::cos_series(r));

    // Rescale the result with the triple angle identity for cosine.
    for(std::uint_fast32_t t = 0U; t < n_angle_identity; ++t)
    {
      c = (((c * c) * c) * 4U) - (c * 3U);
    }

    c = fabs(c);

    if(is_neg)
    {
      c = -c;
    }
  }
  else
  {
    c = floating_point_type(1U);
  }

  return c;
}

} }

namespace local
{
  template<class T>
  bool test_tgamma()
  {
     // N[Gamma[5/2], 120]
     const T control_tgamma_2_and_half("1.32934038817913702047362561250585888709816209209179034616035584238968346344327413603121299255390849906217011771821192800");

     // N[Gamma[5000/2], 120]
     const T control_tgamma_2_fifty   ("1.29314250436364309292832582080974738839793748706951226669917697084512949902204448379552716614841127978037140294127577317E490");

     const T tgamma_2_and_half = boost::math::tgamma(T(T(5)   / 2));
     const T tgamma_2_fifty    = boost::math::tgamma(T(T(500) / 2));

     const T closeness_2_and_half = fabs(1 - fabs(tgamma_2_and_half / control_tgamma_2_and_half));
     const T closeness_2_fifty    = fabs(1 - fabs(tgamma_2_fifty    / control_tgamma_2_fifty));

     const bool result_is_ok_2_and_half = (closeness_2_and_half < std::numeric_limits<T>::epsilon() * T(1.0E5L));
     const bool result_is_ok_2_fifty    = (closeness_2_fifty    < std::numeric_limits<T>::epsilon() * T(1.0E5L));

     const bool result_is_ok = (result_is_ok_2_and_half && result_is_ok_2_fifty);

     return result_is_ok;
  }
}

bool math::wide_decimal::example009b_boost_math_standalone()
{
  using wide_decimal_010_type = math::wide_decimal::decwide_t< 10U, std::uint32_t, void>;
  using wide_decimal_035_type = math::wide_decimal::decwide_t< 35U, std::uint32_t, void>;
  using wide_decimal_105_type = math::wide_decimal::decwide_t<105U, std::uint32_t, void>;

  const bool result_010_is_ok = local::test_tgamma<wide_decimal_010_type>();
  const bool result_035_is_ok = local::test_tgamma<wide_decimal_035_type>();
  const bool result_105_is_ok = local::test_tgamma<wide_decimal_105_type>();

  const bool result_is_ok = (   result_010_is_ok
                             && result_035_is_ok
                             && result_105_is_ok);

  return result_is_ok;
}

// Enable this if you would like to activate this main() as a standalone example.
#if 0

#include <iomanip>
#include <iostream>

int main()
{
  const bool result_is_ok = math::wide_decimal::example009b_boost_math_standalone();

  std::cout << "result_is_ok: " << std::boolalpha << result_is_ok << std::endl;
}

#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic pop
#endif

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#endif