#include <Eigen/Dense>
#include "../include/numerical_gradient.h"

namespace MyDL
{

    using namespace Eigen;

    MatrixXd numerical_gradient(double (*f)(MatrixXd&), MatrixXd& x)
    {
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for (int i = 0; i < size_x; i++)
        {
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            x(i) = tmp_val;
        }

        return grad;
    }

    // TwoLayerNetなどで用いるもの(ラムダ式使用・DNNのインスタンスに含まれるパラメータを参照して使用する場合)
    MatrixXd numerical_gradient(const std::function<double(MatrixXd)> &f, MatrixXd &x)
    {
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for (int i = 0; i < size_x; i++)
        {
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            x(i) = tmp_val;
        }

        return grad;
    }

    MatrixXd numerical_gradient(const std::function<vector<MatrixXd>(MatrixXd)> &f, MatrixXd &X)
    {
        double h = 1e-4;
        int size_X = X.rows() * X.cols();
        MatrixXd grad(X.rows(), X.cols());
        double f_plus_h, f_minus_h;

        for (int i = 0; i < size_X; i++)
        {
            double tmp_val = X(i);

            // f(x + h)
            X(i) = tmp_val + h;
            f_plus_h = f(X)[0].sum();

            // f(x - h)
            X(i) = tmp_val - h;
            f_minus_h = f(X)[0].sum();

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            X(i) = tmp_val;
        }

        return grad;
    }

    MatrixXd numerical_gradient(const std::function<vector<MatrixXd>(VectorXd)> &f, VectorXd &X)
    {
        double h = 1e-4;
        int size_X = X.rows();
        VectorXd grad(X.rows());
        double f_plus_h, f_minus_h;

        for (int i = 0; i < size_X; i++)
        {
            double tmp_val = X(i);

            // f(x + h)
            X(i) = tmp_val + h;
            f_plus_h = f(X)[0].sum();

            // f(x - h)
            X(i) = tmp_val - h;
            f_minus_h = f(X)[0].sum();

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            X(i) = tmp_val;
        }

        return grad;
    }
}


/*
   Oscar Efrain RAMOS PONCE, LAAS-CNRS
   Date: 28/10/2014
   Object to estimate a polynomial that fits some data.
*/

#ifndef _POLY_ESTIMATOR_HH_
#define _POLY_ESTIMATOR_HH_

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

/**
 * Compute the pseudo-inverse using Eigen
 * @param [in] matrix_in is the input matrix.
 * @param [out] pseudo_inv is the pseudo-inverse of the input matrix.
 * @param [in] pinvtoler is the tolerance in the SVD decomposition
 */
void pinv(const Eigen::MatrixXd& matrix_in,
          Eigen::MatrixXd& pseudo_inv,
          const double& pinvtoler = 1.0e-6);

/**
 * Object to fit a polynomial of a given order. It provides a generic fitting
 * polynomial of any order. However, it cannot be used by itself since the
 * proper coefficient of the polynomial (which represents the estimation) needs
 * to be specified. Moreover, the derived classes implement a faster computation
 * based on the specific case.
 *
 */
class PolyEstimator
{

public:

  /**
   * Create a polynomial estimator on a window of length N
   * @param order is the order of the polynomial estimator.
   * @param N is the window length.
   * @param dt is the control (sampling) time
   */
  PolyEstimator(const unsigned int& order,
                const unsigned int& N,
                const double& dt);

  /**
   * Estimate the generic polynomial given a new element. The order of the
   * polynomial is specified in the constructor. Note that this function can be
   * slow if no specialization of fit() has been done, since the generic
   * algorithm would be used.
   * @param [out] estimee is the calculated estimation
   * @param [in] data_element is the new data vector.
   * @param [in] time is the time stamp corresponding to the new data.
   */
  void estimate(std::vector<double>& estimee,
                const std::vector<double>& data_element,
                const double& time);

  /**
   * Estimate the polynomial given a new element assuming a constant time
   * difference. This constant time difference between consecutive samples is
   * given by dt (specified in the constructor). <br>Note: This function will
   * only work if dt is different to zero.
   * @param [out] estimee is the calculated estimation.
   * @param [in] data_element is the new data vector.
   */
  virtual void estimate(std::vector<double>& estimee,
                        const std::vector<double>& data_element) = 0;

  /**
   * Estimate the polynomial given a new element using a recursive
   * algorithm. This method is faster. However, it takes the time as it is (it
   * does not set the lowest time to zero), which can create "ill-conditions".
   * @param [out] estimee is the calculated estimation
   * @param [in] data_element is the new data.
   * @param [in] time is the time stamp corresponding to the new data.
   */
  virtual void estimateRecursive(std::vector<double>& estimee,
                                 const std::vector<double>& data_element,
                                 const double& time) = 0;

  /**
   * Get the time derivative of the estimated polynomial.
   * @param [out] estimeeDerivative is the calculated time derivative.
   * @param [in] order The order of the derivative (e.g. 1 means the first derivative).
   */
  virtual void getEstimateDerivative(std::vector<double>& estimeeDerivative,
                                     const unsigned int order) = 0;

  /**
   * Set the size of the filter window.
   * @param [in] N size
   */
  void setWindowLength(const unsigned int& N);

  /**
   * Get the size of the filter window.
   * @return Size
   */
  unsigned int getWindowLength();

protected:

  /**
   * Find the regressor which best fits in least square sense the last N data
   * sample couples. The order of the regressor is given in the constructor.
   */
  virtual void fit();

  /**
   * Get the estimation when using the generic fit function (in poly-estimator)
   */
  virtual double getEsteeme() = 0;

  /// Order of the polynomial estimator
  unsigned int order_;

  /// Window length
  unsigned int N_;

  /// Sampling (control) time
  double dt_;

  /// Indicate that dt is zero (dt is invalid)
  bool dt_zero_;

  /// Indicate that there are not enough elements to compute. The reason is that
  /// it is one of the first runs, and the estimate will be zero.
  bool first_run_;

  /// All the data (N elements of size dim)
  std::vector< std::vector<double> > elem_list_;

  /// Time vector corresponding to each element in elem_list_
  std::vector< double > time_list_;

  /// Circular index to each data and time element
  unsigned int pt_;

  /// Coefficients for the least squares solution
  Eigen::VectorXd coeff_;

  /// Time vector setting the lowest time to zero (for numerical stability).
  std::vector<double> t_;

  /// Data vector for a single dimension (a single dof). It is only one 'column'
  /// of the elem_list_ 'matrix'
  std::vector<double> x_;

  /// Matrix containing time components. It is only used for the generic fit
  /// computation, such that the estimation is \f$c = R^{\#} x\f$, where \f$x\f$
  /// is the data vector.
  Eigen::MatrixXd R_;

};

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "LinearAlgebra.hpp"

#include <rw/core/macros.hpp>

#include <Eigen/SVD>

using namespace rw::math;

Eigen::MatrixXd LinearAlgebra::pseudoInverse (const Eigen::MatrixXd& am, double precision)
{
    if (am.rows () < am.cols ()) {
        // RW_THROW("pseudoInverse require rows >= to cols!");
        Eigen::MatrixXd a = am.transpose ();
        Eigen::JacobiSVD< Eigen::MatrixXd > svd =
            a.jacobiSvd (Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = precision * std::max (a.cols (), a.rows ()) *
                           svd.singularValues ().array ().abs ().maxCoeff ();
        return (svd.matrixV () *
                Eigen::MatrixXd ((svd.singularValues ().array ().abs () > tolerance)
                                     .select (svd.singularValues ().array ().inverse (), 0))
                    .asDiagonal () *
                svd.matrixU ().adjoint ())
            .transpose ();
    }
    else {
        Eigen::JacobiSVD< Eigen::MatrixXd > svd =
            am.jacobiSvd (Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = precision * std::max (am.cols (), am.rows ()) *
                           svd.singularValues ().array ().abs ().maxCoeff ();
        return svd.matrixV () *
               Eigen::MatrixXd ((svd.singularValues ().array ().abs () > tolerance)
                                    .select (svd.singularValues ().array ().inverse (), 0))
                   .asDiagonal () *
               svd.matrixU ().adjoint ();
    }
}

void LinearAlgebra::svd (const Eigen::MatrixXd& M, Eigen::MatrixXd& U, Eigen::VectorXd& sigma,
                         Eigen::MatrixXd& V)
{
    const Eigen::JacobiSVD< Eigen::MatrixXd > svd =
        M.jacobiSvd (Eigen::ComputeFullU | Eigen::ComputeFullV);
    U     = svd.matrixU ();
    sigma = svd.singularValues ();
    V     = svd.matrixV ();
}

bool LinearAlgebra::checkPenroseConditions (const Eigen::MatrixXd& A, const Eigen::MatrixXd& X,
                                            double prec)
{
    const Eigen::MatrixXd AX = A * X;
    const Eigen::MatrixXd XA = X * A;

    if (((AX * A) - A).lpNorm< Eigen::Infinity > () > prec)
        return false;
    if (((XA * X) - X).lpNorm< Eigen::Infinity > () > prec)
        return false;
    if ((AX.transpose () - AX).lpNorm< Eigen::Infinity > () > prec)
        return false;
    if ((XA.transpose () - XA).lpNorm< Eigen::Infinity > () > prec)
        return false;
    return true;
}

template<>
std::pair< LinearAlgebra::EigenMatrix< double >::type, LinearAlgebra::EigenVector< double >::type >
LinearAlgebra::eigenDecompositionSymmetric< double > (const Eigen::MatrixXd& Am1)
{
    Eigen::SelfAdjointEigenSolver< Eigen::MatrixXd > eigenSolver;
    eigenSolver.compute (Am1);
    return std::make_pair (eigenSolver.eigenvectors (), eigenSolver.eigenvalues ());
}

template<>
std::pair< LinearAlgebra::EigenMatrix< std::complex< double > >::type,
           LinearAlgebra::EigenVector< std::complex< double > >::type >
LinearAlgebra::eigenDecomposition< double > (const Eigen::MatrixXd& Am1)
{
    Eigen::EigenSolver< Eigen::MatrixXd > eigenSolver;
    eigenSolver.compute (Am1);
    return std::make_pair (eigenSolver.eigenvectors (), eigenSolver.eigenvalues ());
}


// Barrier functions that grow to infinity as x -> 0+. Includes gradient and
// hessian functions, too. These barrier functions can be used to impose
// inequlity constraints on a function.

#pragma once

#include <Eigen/Core>

namespace ipc {

/// @brief Function that grows to infinity as x approaches 0 from the right.
///
/// \f$b(d) = -(d-\hat{d})^2\ln\left(\frac{d}{\hat{d}}\right)\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The value of the barrier function at d.
template <typename T> T barrier(const T& d, double dhat);

/// @brief Derivative of the barrier function.
///
/// \f$b'(d) = (\hat{d}-d) \left( 2\ln\left( \frac{d}{\hat{d}} \right) -
/// \frac{\hat{d}}{d} + 1\right)\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The derivative of the barrier wrt d.
double barrier_gradient(double d, double dhat);

/// @brief Second derivative of the barrier function.
///
/// \f$b''(d) = \left( \frac{\hat{d}}{d} + 2 \right) \frac{\hat{d}}{d} -
/// 2\ln\left( \frac{d}{\hat{d}} \right) - 3\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The second derivative of the barrier wrt d.
double barrier_hessian(double d, double dhat);

} // namespace ipc

#include "barrier.tpp"


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::vector<vecteur> matrice;

ENREGISTRER_PROBLEME(82, "Path sum: three ways") {
    // The minimal path sum in the 5 by 5 matrix below, by starting in any cell in the left column and finishing in any
    // cell in the right column, and only moving up, down, and right, is indicated in red and bold; the sum is equal to
    // 994.
    //
    // Find the minimal path sum, in matrix.txt (right click and "Save Link/Target As..."), a 31K text file containing
    // a 80 by 80 matrix, from the left column to the right column.
    matrice graphe;
    std::ifstream ifs("data/p082_matrix.txt");
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(","));
        vecteur l;
        for (const auto &s: v) {
            l.push_back(std::stoull(s));
        }
        graphe.push_back(std::move(l));
    }

    const nombre taille = graphe.size();
    matrice chemin(taille, vecteur(taille, 0));
    for (size_t i = 0; i < taille; ++i) {
        vecteur chemin_bas(taille, 0);
        for (size_t j = 0; j < taille; ++j) {
            if (i == 0)
                chemin_bas[j] = graphe[j][i];
            else if (j == 0)
                chemin_bas[j] = chemin[j][i - 1] + graphe[j][i];
            else
                chemin_bas[j] = std::min(chemin[j][i - 1], chemin_bas[j - 1]) + graphe[j][i];
        }

        vecteur chemin_haut(taille, 0);
        for (size_t jj = 0; jj < taille; ++jj) {
            const size_t j = taille - jj - 1;
            if (i == 0)
                chemin_haut[j] = graphe[j][i];
            else if (j == taille - 1)
                chemin_haut[j] = chemin[j][i - 1] + graphe[j][i];
            else
                chemin_haut[j] = std::min(chemin[j][i - 1], chemin_haut[j + 1]) + graphe[j][i];
        }

        for (size_t j = 0; j < taille; ++j)
            chemin[j][i] = std::min(chemin_haut[j], chemin_bas[j]);
    }

    nombre resultat = std::numeric_limits<nombre>::max();
    for (const auto &c: chemin) {
        resultat = std::min(resultat, c.back());
    }

    return std::to_string(resultat);
}


#include <iostream>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <math.h>
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using namespace std;

struct Coordinate
{
	double x, y, z;

	Coordinate(double x, double y, double z)
	{
		this->x = x;
		this->y = y;
		this->z = z;
	}
};

struct Rotation
{
	double r, p, y;

	Rotation(double roll, double pitch, double yall)
	{
		r = roll;
		p = pitch;
		y = yall;
	}
};

#define D 3

Matrix3d RotationMatrix(float, float, float);
Matrix4d HomogeneousTransformationMatrix(Coordinate, Rotation);
Matrix4d TranslationMatrix(Coordinate);

int main()
{
	Matrix3d R;
	Matrix4d T;

	T = HomogeneousTransformationMatrix(Coordinate(4,5,6), Rotation(0, 0, 0));

	cout << T << endl;
}

Matrix3d RotationX(float thetax)
{
	Matrix3d Rx;

	Rx << 1, 0, 0,
		  0, cos(thetax), -sin(thetax),
		  0, sin(thetax), sin(thetax);

	return Rx;
}

Matrix3d RotationY(float thetay)
{
	Matrix3d Ry;

	Ry << cos(thetay), 0, sin(thetay),
		  0, 1, 0,
		  -sin(thetay), 0, cos(thetay);

	return Ry;
}

Matrix3d RotationZ(float thetaz)
{
	Matrix3d Rz;

	Rz << cos(thetaz), -sin(thetaz), 0,
		  sin(thetaz), cos(thetaz), 0,
		  0, 0, 1;

	return Rz;
}

Matrix3d RotationMatrix(float thetaz1, float thetay, float thetaz2) // Angle in degrees
{
	return RotationZ(thetaz1)*RotationY(thetay)*RotationZ(thetaz2);
}

Matrix4d HomogeneousTransformationMatrix(Coordinate origin, Rotation rot)
{
	Matrix4d T;

	T.block(0, 0, 3, 3) << RotationMatrix(rot.r, rot.p, rot.y);
	T.block(0, 3, 3, 1) << origin.x, origin.y, origin.z;
	T.block(3, 0, 1, 3) << MatrixXd::Zero(1,3);
	T.block(3, 3, 1, 1) << MatrixXd::Identity(1,1);

	return T;
}

Matrix4d TranslationMatrix(Coordinate origin)
{
	return HomogeneousTransformationMatrix(origin, Rotation(0,0,0));
}


#include <math.h>
#include "config.h"
#include "matrix.h"
#include "util.h"
#include <armadillo>

void normalize(float *x, float *y, float *z) {
    float d = sqrtf((*x) * (*x) + (*y) * (*y) + (*z) * (*z));
    *x /= d; *y /= d; *z /= d;
}

void copy_matrix(float *dst, const arma::mat& src){
	for(int i = 0; i < 4; i++){
		for(int j = 0; j < 4; j++){
			dst[i * 4 + j] = src(i,j);
		}
	}
}

arma::mat copy_from_array(float *src){
	arma::mat dst(4,4);
	for(int i = 0; i < 4; i++){
		for(int j = 0; j < 4; j++){
			dst(i,j) = src[i * 4 + j];
		}
	}
	return dst;
}

arma::mat mat_translate(float dx, float dy, float dz) {
    arma::mat translate(4,4, arma::fill::zeros);
    translate.eye();
    translate(3,0) = dx;
    translate(3,1) = dy;
    translate(3,2) = dz;
    return translate;
}

arma::mat mat_rotate(float x, float y, float z, float t) {
    float ux = x;
    float uy = y;
    float uz = z;
    normalize(&ux, &uy, &uz);
    return arma::mat {
            { cos(t)+(ux*ux)*(1-cos(t)), (ux*uy)*(1-cos(t))-uz*sin(t), ux*uz*(1-cos(t)) + uy*sin(t), 0 },
            { uy*uz*(1-cos(t))+uz*sin(t), cos(t)+(uy*uy)*(1-cos(t)), uy*uz*(1-cos(t)) - ux*sin(t), 0},
            { uz*ux*(1-cos(t))-uy*sin(t), uz*uy*(1-cos(t)) + ux*sin(t), cos(t) + uz*uz*(1 - cos(t)), 0 },
            { 0, 0, 0, 1}
    };
}

void mat_apply(std::vector<float>& d, arma::mat &ma, int count, int offset, int stride) {
	arma::mat vec = {0,0,0,1};
	for (int i = 0; i < count; i++) {
        int cursor = offset + stride * i;
		vec(0,0) = d.at(cursor++);
		vec(0,1) = d.at(cursor++);
		vec(0,2) = d.at(cursor++);

		vec = vec * ma;

		cursor = offset + stride * i;
		d.at(cursor++) = vec(0,0);
		d.at(cursor++) = vec(0,1);
		d.at(cursor++) = vec(0,2);
	}
}

arma::mat frustum_planes(int radius, float *matrix) {
    arma::mat planes(6,4);
    float znear = 0.125;
    float zfar = radius * 32 + 64;
    float *m = matrix;
    planes(0,0) = m[3] + m[0];
    planes(0,1) = m[7] + m[4];
    planes(0,2) = m[11] + m[8];
    planes(0,3) = m[15] + m[12];
    planes(1,0) = m[3] - m[0];
    planes(1,1) = m[7] - m[4];
    planes(1,2) = m[11] - m[8];
    planes(1,3) = m[15] - m[12];
    planes(2,0) = m[3] + m[1];
    planes(2,1) = m[7] + m[5];
    planes(2,2) = m[11] + m[9];
    planes(2,3) = m[15] + m[13];
    planes(3,0) = m[3] - m[1];
    planes(3,1) = m[7] - m[5];
    planes(3,2) = m[11] - m[9];
    planes(3,3) = m[15] - m[13];
    planes(4,0) = znear * m[3] + m[2];
    planes(4,1) = znear * m[7] + m[6];
    planes(4,2) = znear * m[11] + m[10];
    planes(4,3) = znear * m[15] + m[14];
    planes(5,0) = zfar * m[3] - m[2];
    planes(5,1) = zfar * m[7] - m[6];
    planes(5,2) = zfar * m[11] - m[10];
    planes(5,3) = zfar * m[15] - m[14];
    return planes;
}

arma::mat mat_frustum(float left, float right, float bottom, float top, float znear, float zfar)
{
    float temp, temp2, temp3, temp4;
    temp = 2.0 * znear;
    temp2 = right - left;
    temp3 = top - bottom;
    temp4 = zfar - znear;
    return arma::mat {
        {temp / temp2, 0.0, 0.0, 0.0},
        {0.0, temp / temp3, 0.0, 0.0},
        {(right + left) / temp2, (top + bottom) / temp3, (-zfar - znear) / temp4, -1.0},
        {0.0, 0.0, (-temp * zfar) / temp4, 0.0}
    };
}

arma::mat mat_perspective(float fov, float aspect, float znear, float zfar) {
    float ymax, xmax;
    ymax = znear * tanf(fov * PI / 360.0);
    xmax = ymax * aspect;
    return mat_frustum(-xmax, xmax, -ymax, ymax, znear, zfar);
}

arma::mat mat_ortho(float left, float right, float bottom, float top, float near, float far) {
    return arma::mat {
        {2 / (right - left), 0, 0, 0},
        {0, 2 / (top - bottom), 0, 0},
        {0, 0, -2 / (far - near), 0},
        {-(right + left) / (right - left), -(top + bottom) / (top - bottom), -(far + near) / (far - near), 1}
    };
}

arma::mat set_matrix_2d(int width, int height) {
    return mat_ortho(0, width, 0, height, -1, 1);
}

arma::mat set_matrix_3d(int width, int height,
        float x, float y, float z, float rx, float ry,
        float fov, int ortho, int radius){

    arma::mat a(4,4);
    arma::mat b(4,4);
    float aspect = (float)width / height;
    float znear = 0.125;
    float zfar = radius * 32 + 64;
    a.eye();
    b = mat_translate(-x, -y, -z);
    a = a * b;
    b = mat_rotate(cosf(rx), 0, sinf(rx), ry);
    a = a * b;
    b = mat_rotate(0, 1, 0, -rx);
    a = a * b;
    if (ortho) {
        int size = ortho;
        b = mat_ortho(-size * aspect, size * aspect, -size, size, -zfar, zfar);
    }
    else {
        b = mat_perspective(fov, aspect, znear, zfar);
    }
    a = a * b;
    arma::mat m(4,4);
    m.eye();
    m = m* a;
    return m;
}

arma::mat set_matrix_item(int width, int height, int scale) {
    arma::mat a(4,4);
    arma::mat b(4,4);
    float aspect = (float)width / height;
    float size = 64 * scale;
    float box = height / size / 2;
    float xoffset = 1 - size / width * 2;
    float yoffset = 1 - size / height * 2;
    a.eye();
    b = mat_rotate(0, 1, 0, -PI / 4);
    a = a * b;
    b = mat_rotate(1, 0, 0, -PI / 10);
    a = a * b;
    b = mat_ortho(-box * aspect, box * aspect, -box, box, -1, 1);
    a = a * b;
    b = mat_translate(-xoffset, -yoffset, 0);
    a = a * b;
    arma::mat m(4,4);
    m.eye();
    m = m * a;
    return m;
}


//
// Created by chen-tian on 17-7-4.
//
#include <iostream>
#include <cmath>
using namespace std;

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

int main(int argc, char** argv)
{
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d(0,0,1));//沿z轴旋转45度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix()<<endl;
    rotation_matrix = rotation_vector.toRotationMatrix();
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector*v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    v_rotated=rotation_matrix*v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    //euler Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//although it is called 3d, this matrix is defeined by 4*4
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;

    Eigen::Vector3d v_transformed = T*v;
    cout<<"v transformed = \n"<<v_transformed.transpose()<<endl;

    //quaterniond
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;
    q=Eigen::Quaterniond (rotation_matrix);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;
    v_rotated = q*v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
};


#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <math.h>
#include <set>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/number.hpp>

using namespace std;
using namespace boost::multiprecision;

int target = 5;

unsigned long long power(int n, int k) {
  unsigned long long sum = 1;
  for (int i = 0; i < k; i++) {
    sum *= n;
  }
  return sum;
}

unsigned long long digits_power_sum(int n) {
  unsigned long long sum = 0;
  int digit;
  while (n) {
    digit = n % 10;
    n /= 10;

    sum += power(digit, target);
  }
  return sum;
}

int main(int argc, char** argv) {
  int max = power(10, target + 1) - 1;
  unsigned long long sum = 0;
  for (int i = 2; i < max; i++) {
    if (digits_power_sum(i) == i) {
      cout << i << " can be written as the sum of its " << target << "th power sums " << endl;
      sum += i;
    }
  }
  cout << "Sum of all " << target << "th-powerable integers is: " << sum << endl;
  return 0;
}


#include "utils.hpp"
#include <cmath>
#include <ctime>
#include <fstream>
#include <boost/range/numeric.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/median.hpp>

using namespace std;

namespace delphi::utils {

/**
 * Returns the value rounded to n places past the decimal
 */
double round_n(double x, int places) {
  double n = pow(10.0, places);
  return (double)((int)(x * n + 0.5))/n;
}

/**
 * Returns the square of a number.
 */
double sqr(double x) { return x * x; }

/**
 * Returns the sum of a vector of doubles.
 */
double sum(const std::vector<double> &v) { return boost::accumulate(v, 0.0); }

/**
 * Returns the arithmetic mean of a vector of doubles.
 * Updated based on:
 * https://codereview.stackexchange.com/questions/185450/compute-mean-variance-and-standard-deviation-of-csv-number-file
 */
double mean(const std::vector<double> &v) {
    if (v.empty()) {
        return std::numeric_limits<double>::quiet_NaN();
    }

    return sum(v) / v.size();
}

/**
 * Returns the sample standard deviation of a vector of doubles.
 * Based on:
 * https://codereview.stackexchange.com/questions/185450/compute-mean-variance-and-standard-deviation-of-csv-number-file
 */
double standard_deviation(const double mean, const std::vector<double>& v)
{
    if (v.size() <= 1u)
        return std::numeric_limits<double>::quiet_NaN();

    auto const add_square = [mean](double sum, int i) {
        auto d = i - mean;
        return sum + d*d;
    };
    double total = std::accumulate(v.begin(), v.end(), 0.0, add_square);
    return sqrt(total / (v.size() - 1));
}

/**
 * Returns the median of a vector of doubles.
 */
double median(const std::vector<double> &xs) {
    if (xs.size() > 100) {
        using namespace boost::accumulators;
        accumulator_set<double, features<tag::median>> acc;
        //  accumulator_set<double,
        //      features<tag::median(with_p_square_cumulative_distribution) >>
        //      acc ( p_square_cumulative_distribution_num_cells = xs.size() );

        for (auto x : xs) {
          acc(x);
        }

        return boost::accumulators::median(acc);
    } else {
        vector<double> x_copy(xs);
        sort(x_copy.begin(), x_copy.end());
        int num_els = x_copy.size();
        int mid = num_els / 2;
        if (num_els % 2 == 0) {
            return (x_copy[mid - 1] +  x_copy[mid]) / 2;
        }
        else {
            return x_copy[mid];
        }
    }
}

/**
 * Returns the center absolute deviation of a vector of doubles.
 * Based on:
 * https://en.wikipedia.org/wiki/Median_absolute_deviation
 */
double median_absolute_deviation(const double center, const std::vector<double>& v)
{
  std::vector<double> abs_diff = std::vector<double>(v.size());

  transform(v.begin(), v.end(),
            abs_diff.begin(),
            [&](double val){return abs(center - val);});

  return median(abs_diff);
}

double log_normpdf(double x, double mean, double sd) {
  double var = pow(sd, 2);
  double log_denom = -0.5 * log(2 * M_PI) - log(sd);
  double log_nume = pow(x - mean, 2) / (2 * var);

  return log_denom - log_nume;
}

nlohmann::json load_json(string filename) {
  ifstream i(filename);
  nlohmann::json j = nlohmann::json::parse(i);
  return j;
}

/** Compute the number of months between two dates **/
int months_between(tuple<int, int, int> earlier_date, tuple<int, int, int> latter_date) {
  int earlier_year = get<0>(earlier_date);
  int earlier_month = get<1>(earlier_date);
  int latter_year = get<0>(latter_date);
  int latter_month = get<1>(latter_date);

  return 12 * (latter_year - earlier_year) + (latter_month - earlier_month);
}

std::string get_timestamp() {
  time_t now = time(0);

  struct tm *ptm = localtime(&now);
  int year = 1900 + ptm->tm_year;
  int month = 1 + ptm->tm_mon;
  int date = ptm->tm_mday;
  int hour = ptm->tm_hour;
  int minute = ptm->tm_min;
  int second = ptm->tm_sec;

  return to_string(year) + "-" +
         to_string(month) + "-" +
         to_string(date) + "_" +
         to_string(hour) + "." +
         to_string(minute) + "." +
         to_string(second);
}
} // namespace delphi::utils


#include <iostream>
#include <chrono>
#include <vector>
#include <numeric>
#include <algorithm>
#include <boost/iterator/counting_iterator.hpp>

//bad style do-while and wrong for Factorial1(0LL) -> 0 !!!
long long int Factorial1(long long int m_nValue)
{
   long long int result=m_nValue;
   long long int result_next;
   long long int pc = m_nValue;
   do
   {
       result_next = result*(pc-1);
       result = result_next;
       pc--;
   }while(pc>2);
   m_nValue = result;
   return m_nValue;
}

//iteration with while
long long int Factorial2(long long int n)
{
   long long int r = 1;
   while(1<n)
       r *= n--;
   return r;
}

//recrusive
long long int Factorial3(long long int n)
{
   return n<2 ? 1 : n*Factorial3(n-1);
}

//tail recursive
inline long long int _fac_aux(long long int n, long long int acc) {
    return n < 1 ? acc : _fac_aux(n - 1, acc * n);
}
long long int Factorial4(long long int n)
{
   return _fac_aux(n,1);
}

//accumulate with functor
long long int Factorial5(long long int n)
{
  // last is one-past-end
  return std::accumulate(boost::counting_iterator<long long int>(1LL),
                         boost::counting_iterator<long long int>(n+1LL), 1LL,
                         std::multiplies<long long int>() );
}

//accumulate with lamda
long long int Factorial6(long long int n)
{
  // last is one-past-end
  return std::accumulate(boost::counting_iterator<long long int>(1LL),
                         boost::counting_iterator<long long int>(n+1LL), 1LL,
                         [](long long int a, long long int b) { return a*b; } );
}

int main()
{
    int v = 55;
    {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto result = Factorial1(v);
        auto t2 = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> ms = t2 - t1;
        std::cout << std::fixed << "do-while(1)              result " << result
                  << " took " << ms.count() << " ms\n";
    }

    {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto result = Factorial2(v);
        auto t2 = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> ms = t2 - t1;
        std::cout << std::fixed << "while(2)                 result " << result
                  << " took " << ms.count() << " ms\n";
    }

    {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto result = Factorial3(v);
        auto t2 = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> ms = t2 - t1;
        std::cout << std::fixed << "recusive(3)              result " << result
                  << " took " << ms.count() << " ms\n";
    }

    {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto result = Factorial3(v);
        auto t2 = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> ms = t2 - t1;
        std::cout << std::fixed << "tail recusive(4)         result " << result
                  << " took " << ms.count() << " ms\n";
    }

    {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto result = Factorial5(v);
        auto t2 = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> ms = t2 - t1;
        std::cout << std::fixed << "std::accumulate(5)       result " << result
                  << " took " << ms.count() << " ms\n";
    }

    {
        auto t1 = std::chrono::high_resolution_clock::now();
        auto result = Factorial6(v);
        auto t2 = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> ms = t2 - t1;
        std::cout << std::fixed << "std::accumulate lamda(6) result " << result
                  << " took " << ms.count() << " ms\n";
    }
}


#include "tut/ch3.hpp"

#include <math.h>
#include <cmath>
#include <random>
#include <ctime>
#include <iostream>
#include <chrono>

#include "boost/date_time/gregorian/gregorian.hpp"
#include <boost/log/trivial.hpp>

using namespace boost::gregorian;

namespace tut {
  namespace ch3 {
    double exe_3_01(double a, double b, double c);

    double FirstRoot(double a, double b, double c)
    {
      double r1 {(-b + sqrt(exe_3_01(a,b,c))) / (2 * a) };
      BOOST_LOG_TRIVIAL(debug) << "The first root: " << r1;
      return std::round( (r1 / 100000) * 10000000000 ) / 100000 ;
    }
    double SecondRoot(double a, double b, double c) {
      double r2 { (-b - sqrt(exe_3_01(a,b,c))) / (2 * a)};
      BOOST_LOG_TRIVIAL(debug) << "The second root: " << r2;
      return std::round( (r2 / 100000) * 10000000000 ) / 100000;
    }

    double exe_3_01(double a, double b, double c) {
      double discriminant { pow(b, 2) - (4 * a * c)};
      discriminant = std::round( (discriminant / 100000) * 10000000000 ) / 100000;
      if (discriminant > 0 ) {
          BOOST_LOG_TRIVIAL(debug) << "The equation has 2 real roots\n";
          return discriminant;
        }
      if (discriminant == 0) {
          BOOST_LOG_TRIVIAL(debug) << "The equation has one root\n";
          return discriminant;
        }
      if (discriminant < 0) {
          BOOST_LOG_TRIVIAL(debug) << "The equation has no real roots\n";
          return discriminant;
        }
      return discriminant;
    }

    void exe_3_02() {
      // Initialize our mersenne twister with a random seed based on the clock
      std::mt19937 mersenne{ static_cast<std::mt19937::result_type>(std::time(nullptr)) };
      // Create a reusable random number generator that generates uniform numbers between 1 and 6
      std::uniform_int_distribution random_number{ 1, 10 };
      int num1 {random_number(mersenne)};
      int num2 {random_number(mersenne)};
      int num3 {random_number(mersenne)};
      std::cout << "what is : " << num1 << " + " << num2 << " + " << num3 <<"\n";
      std::cout << "Enter your answer: ";
      int answer{0};
      std::cin >> answer;
      int sum{ num1 + num2 + num3 };
      if (answer == sum ) {
          std::cout << "Correct , the answer is: "<< sum <<"\n";
        } else {
          std::cout << "Incorrect , the answer is: " << sum << "\n";
        }
    }

    std::vector<double> exe_3_03(double x1, double y1, double z1, double x2, double y2,double z2) {
      double x {( ((z1*y2) - (y1*z2)) / ((x1*y2) - (y1*x2)) )};
      x = std::round( (x / 10) * 100 ) / 10;
      double y { ( ((x1*z2) - (z1*x2)) / ((x1*y2) - (y1*x1)) ) };
      y = std::round( (y / 10) * 100 ) / 10;
      std::vector<double> sol;
      double exp {x1*y2 - y1*x2};
      if (exp == 0) {
          BOOST_LOG_TRIVIAL(debug) << "The equation has no solution \n";
          BOOST_LOG_TRIVIAL(debug) << "x is : " << x << " and y is: " << y << "\n";
          sol.push_back(x);
          sol.push_back(y);
          return sol;
        } else {
          BOOST_LOG_TRIVIAL(debug) << "x is : " << x << " and y is: " << y << "\n";
          sol.push_back(x);
          sol.push_back(y);
          return sol;
        }
      return sol;
    }

    void exe_3_04() {
      // Initialize our mersenne twister with a random seed based on the clock
      std::mt19937 mersenne{ static_cast<std::mt19937::result_type>(std::time(nullptr)) };
      // Create a reusable random number generator that generates uniform numbers between 1 and 6
      std::uniform_int_distribution random_number{ 1, 12 };
      BOOST_LOG_TRIVIAL(debug) << "The english month name: " << greg_month(random_number(mersenne));
    }

    void exe_3_05() {
      std::cout << "Enter today's day[ 0 - 6]: ";
      int today;
      std::cin >> today;
      if (today > 6) {
          std::cout << "Incorrect date, enter again: ";
          std::cin >> today;
        }
      std::cout << "Enter the number of days elapsed since today[1 - 31]: ";
      int elapsed_days;
      std::cin >> elapsed_days;
      if (elapsed_days > 31) {
          std::cout << "Incorrect date, enter again: ";
          std::cin >> elapsed_days;
        }
      days future_day{ days{today} + days {elapsed_days} };
      /// todo:: solution incomplete.
      std::cout << "Today is " << greg_weekday(today) << " and the future day is: "<< future_day.days() <<std::endl;
      BOOST_LOG_TRIVIAL(warning) << " The furture date is incorrect.\n";
    }

    void exe_3_06() {
      std::cout << "Enter the weight in pounds: ";
      double weight{0.0};
      std::cin >> weight;
      std::cout << "Enter feet: ";
      double feet{0.0};
      std::cin >> feet;
      std::cout << "Enter inches: ";
      double height{0.0};
      std::cin >> height;

      constexpr double KILOGRAMS_PER_POUND = 0.45359237; // Constant
      constexpr double METERS_PER_INCH = 0.0254;

      double weightInKilograms = weight * KILOGRAMS_PER_POUND;
      double heightInMeters = height * METERS_PER_INCH;
      double bmi = weightInKilograms /
          (heightInMeters * heightInMeters);

      std::cout << "BMI is : " << bmi << std::endl;
      if (bmi < 18.5) {
          std::cout << "Underweight\n";
        } else if(bmi < 25) {
          std::cout << "Normal\n";
        } else if(bmi < 30) {
          std::cout << "Overweight\n";
        } else {
          std::cout << "Obese\n";
        }
    }

    void exe_3_07() {
      std::cout << "Enter an amount, for example 11.56:";
      double ammount;
      std::cin >> ammount;
      int remainingAmmount{ static_cast<int>(ammount * 100)};
      int numberOfDollars { static_cast<int>(remainingAmmount / 100) };
      remainingAmmount = remainingAmmount % 100;
      int numberOfQuarters = remainingAmmount / 25;
      remainingAmmount = remainingAmmount % 25;
      int numberOfDimes = remainingAmmount / 10;
      remainingAmmount = remainingAmmount % 10;
      int numberOfNickels = remainingAmmount / 5;
      remainingAmmount = remainingAmmount % 5;
      int numberOfPenies = remainingAmmount;
      std::cout << "The ammount " << ammount << std::endl;

      if (numberOfDollars > 0) {
          std::cout << "Number of dollars: " << numberOfDollars << "s\n";
        } else {
          std::cout << "Number of dollar: " << numberOfDollars << std::endl;
        }
      if (numberOfQuarters > 0) {
          std::cout << "Number of quarters: " << numberOfQuarters << "s\n";
        } else {
          std::cout << "Number of quarter: " << numberOfQuarters << std::endl;
        }
      if (numberOfDimes > 0) {
          std::cout << "Number of dimes: " << numberOfDimes << "s\n";
        } else {
          std::cout << "Number of dime: " << numberOfDimes << std::endl;
        }
      if (numberOfNickels > 0) {
          std::cout << "Number of nickels: " << numberOfNickels << "s\n";
        } else {
          std::cout << "Number of nickel: " << numberOfNickels << std::endl;
        }
      if (numberOfPenies > 0) {
          std::cout << "Number of penies: " << numberOfPenies << "s\n";
        } else {
          std::cout << "Number of penies: " << numberOfPenies << std::endl;
        }

    }

    void exe_3_08() {
      int numbers, thousands, hundreds, tens;
      std::cout << "Enter the 3 numbers: ";
      std::cin >> numbers; // 432

      tens = numbers % 10; // 2
      int hundredsAndThousands = numbers / 10; // 43
      hundreds = hundredsAndThousands % 10; // 3
      thousands = hundredsAndThousands / 10;
      BOOST_LOG_TRIVIAL(debug) << "Thousands: " << thousands << " hundreds: " << hundreds << " tens: " << tens <<"\n";
      if (thousands == hundreds && hundreds == tens
          && thousands == tens) {
          std::cout << "Numbers already sorted\n";
        }
      std::vector<int> sort; // 345 435
      int temp;
      if (hundreds < thousands || tens < thousands) // 259
        {
          if (hundreds < thousands)
            {
              temp = thousands;
              thousands = hundreds;
              hundreds = temp;
            }
          if (tens < thousands)
            {
              temp = thousands;
              thousands = tens;
              tens = temp;
            }
        }
      if (tens < hundreds)
        {
          temp = hundreds;
          hundreds = tens;
          tens = temp;
        }
      std::cout << "Sort : " << tens << " " << hundreds << " " << thousands << std::endl;
    }

    void exe_3_09()
    {
      std::cout << "Enter the first 9 digits of an ISBN: ";
      int isbn{0};
      std::cin >> isbn;
      int d9{ isbn % 10 };
      int rem {isbn / 10};
      int d8{ rem % 10 };
      rem = rem / 10;
      int d7{ rem % 10 };
      rem =rem / 10;
      int d6{rem % 10};
      rem = rem / 10;
      int d5{rem % 10};
      rem = rem / 10;
      int d4{rem % 10};
      rem = rem / 10;
      int d3{rem % 10};
      rem = rem / 10;
      int d2{ rem % 10};
      rem = rem /10;
      int d1{ rem };
      int d10{ (d1*1) + (d2*2) + (d3*3) + (d4*4) + (d5*5) + (d6*6) + (d7*7) + (d8*8) + (d9*9) };
      d10 = d10 % 11;

      std::string s{ std::to_string(isbn)};

      if (d10 == 10) {
          s.append("X");
          std::cout << "The ISBN-10 number is: "<< s << std::endl;
        } else {
          s.append(std::to_string(d10));
          std::cout << "The ISBN-10 number is: "<< s << std::endl;
        }
    }

    void exe_3_10() {
      auto seed = std::chrono::system_clock::now().time_since_epoch().count();
      auto mtgen = std::mt19937{static_cast<unsigned int>(seed) };
      auto ud = std::uniform_int_distribution<>{1, 100};
      auto number1 = ud(mtgen);
      auto number2 = ud(mtgen);
      std::cout << "What is " << number1 << " + " << number2 << "? ";
      int answer{0};
      std::cin >> answer;
      if (number1+number2 == answer) {
          std::cout << "You are correct !\n";
        } else {
          std::cout << "Your answer is wrong, "<< number1 << " + " << number2 << " should be " << number1 + number2 << std::endl;

        }
    }

    void exe_3_11() {
      std::cout << "Enter a month [1 -12]: ";
      int month{0};
      std::cin >> month;
      std::cout << "Enter a year, i,e 2022: ";
      int year;
      std::cin >> year;
      using namespace boost::gregorian;
      auto end_of_month_day = gregorian_calendar::end_of_month_day(year, month);
      std::cout << " "<< greg_month(month) << " had " << end_of_month_day << " days." << std::endl;
    }

    void exe_3_12() {
      std::cout << "Enter a three digit integer: ";
      int number;
      std::cin >> number;
      int number3 { number % 10 } ;
      int rem { rem / 10 };
      int number2{ rem % 10 };
      rem =rem / 10;
      int number1{ rem % 10 };
      if (number1 == number3) {
          std::cout << " " << number << " is a palindrome\n";
        } else {
          std::cout << number << " is not a palindrome\n";
        }

    }

    void exe_3_13() {

      // Prompt the user to enter filing status
      std::cout << "(0-single filter, 1-married jointly or " <<
                   "qualifying widow(er), 2-married separately, 3-head of " <<
                   "houshold) Enter the filing status: ";
      int status{0};
      std::cin >> status;

      // Prompt the user to enter taxable income
      std::cout << "Enter the taxable income: ";
      double income{0};
      std::cin >> income;

      // Compute tax
      double tax = 0;
      switch (status)
        {
        case 0 : // Compute tax for single filers
          tax += (income <= 8350) ? income * 0.10 : 8350 * 0.10;
          if (income > 8350)
            tax += (income <= 33950) ? (income - 8350) * 0.15 :
                                       25600 * 0.15;
          if (income > 33950)
            tax += (income <= 82250) ? (income - 33950) * 0.25 :
                                       48300 * 0.25;
          if (income > 82250)
            tax += (income <= 171550) ? (income - 82250) * 0.28 :
                                        89300 * 0.28;
          if (income > 171550)
            tax += (income <= 372950) ? (income - 171550) * 0.33 :
                                        201400 * 0.33;
          if (income > 372950)
            tax += (income - 372950) * 0.35;
          break;
        case 1 : // Compute tax for married file jointly or qualifying widow(er)
          tax += (income <= 16700) ? income * 0.10 : 16700 * 0.10;
          if (income > 16700)
            tax += (income <= 67900) ? (income - 16700) * 0.15 :
                                       (67900 - 16700) * 0.15;
          if (income > 67900)
            tax += (income <= 137050) ? (income - 67900) * 0.25 :
                                        (137050 - 67900) * 0.25;
          if (income > 137050)
            tax += (income <= 208850) ? (income - 137050) * 0.28 :
                                        (208850 - 137050) * 0.28;
          if (income > 208850)
            tax += (income <= 372950) ? (income - 208850) * 0.33 :
                                        (372950 - 208850) * 0.33;
          if (income > 372950)
            tax += (income - 372950) * 0.35;
          break;
        case 2 : // Compute tax for married separately
          tax += (income <= 8350) ? income * 0.10 : 8350 * 0.10;
          if (income > 8350)
            tax += (income <= 33950) ? (income - 8350) * 0.15 :
                                       (33950 - 8350) * 0.15;
          if (income > 33950)
            tax += (income <= 68525) ? (income - 33950) * 0.25 :
                                       (68525 - 33950) * 0.25;
          if (income > 68525)
            tax += (income <= 104425) ? (income - 68525) * 0.28 :
                                        (104425 - 68525) * 0.28;
          if (income > 104425)
            tax += (income <= 186475) ? (income - 104425) * 0.33 :
                                        (186475 - 104425) * 0.33;
          if (income > 186475)
            tax += (income - 186475) * 0.35;
          break;
        case 3 : // Compute tax for head of household
          tax += (income <= 11950) ? income * 0.10 : 11950 * 0.10;
          if (income > 11950)
            tax += (income <= 45500) ? (income - 11950) * 0.15 :
                                       (45500 - 11950) * 0.15;
          if (income > 45500)
            tax += (income <= 117450) ? (income - 45500) * 0.25 :
                                        (117450 - 45500) * 0.25;
          if (income > 117450)
            tax += (income <= 190200) ? (income - 117450) * 0.28 :
                                        (190200 - 117450) * 0.28;
          if (income > 190200)
            tax += (income <= 372950) ? (income - 190200) * 0.33 :
                                        (372950 - 190200) * 0.33;
          if (income > 372950)
            tax += (income - 372950) * 0.35;
          break;
        default : std::cout << "Error: invalid status\n";
          quick_exit(1);
        }
      // Display the result
      std::cout << "Tax is " << (int)(tax * 100) / 100.0 << std::endl;
    }

    void exe_3_14() {
      auto seed = std::chrono::system_clock::now().time_since_epoch().count();
      auto mtgen = std::mt19937{static_cast<unsigned int>(seed)};
      auto ud = std::uniform_int_distribution<>{0, 1};
      auto coin_side { ud(mtgen) };
      std::cout << "Guess the coin flip [ 0= heads, 1=Tails ]: ";
      int guess{0};
      std::cin >> guess;

      if (guess == 0 && coin_side == 0) {
          std::cout << "Correct guess, Heads\n";
        } else {
          std::cout << "Incorrect guess, Tails\n";
        }
    }

    std::string exe_3_15(int digits) {
      auto seed {std::chrono::steady_clock::now().time_since_epoch().count()};
      auto mtgen {std::mt19937{static_cast<unsigned int >(seed)}};
      auto ud {std::uniform_int_distribution<>(100, 999)};

      std::string response1{"Exact match: you win $10,000"};
      std::string response2{"Match all digits: you win $3,000"};
      std::string response3{"Match one digit: you win $1,000"};
      std::string response4{"Sorry, no match"};

      int lottery { ud(mtgen) }; // genereate the lottery number
      int lottery1{}, lottery2{}, lottery3{}, lrem{};
      lottery3 = lottery % 10;
      lrem = lottery / 10;
      lottery2 = lrem % 10;
      lottery3 = lrem / 10;

      int guess1{}, guess2{}, guess3{}, rem{};
      guess3 = digits % 10;
      rem = digits / 10;
      guess2 = rem % 10;
      guess1 = rem / 10;

      if (guess1 == lottery1 && guess2 == lottery2 && guess3 == lottery3) {
          BOOST_LOG_TRIVIAL(debug) << "Lottery: "<< lottery << " guess: "<< response1;
          return response1;
        } else if(digits == lottery) {
          BOOST_LOG_TRIVIAL(debug) << "Lottery: "<< lottery << " guess: "<< response2;
          return response2;
        } else if(guess1 == lottery1 || guess2 == lottery2 || guess3 == lottery3) {
          BOOST_LOG_TRIVIAL(debug) << "Lottery: "<< lottery << " guess: "<< response3;
          return response3;
        } else {
          BOOST_LOG_TRIVIAL(debug) << "Lottery: "<< lottery << " guess: "<< response4;
          return response4;
        }

      return response4;
    }

    void exe_3_16() {
      auto seed = std::chrono::system_clock::now().time_since_epoch().count();
      auto mtgen = std::mt19937{ static_cast<unsigned int >(seed)};
      auto ud = std::uniform_int_distribution<>(0, 100);
      auto ud1 = std::uniform_int_distribution<>(0, 200);
      int x{ ud(mtgen) }, y{ ud1(mtgen) };
      std::cout << "The random coordinate of the recatangle start: x1=" << x << " y1=" << y << std::endl;
    }

    void exe_3_17() {
      auto seed = std::chrono::system_clock::now().time_since_epoch().count();
      auto mtgen = std::mt19937{static_cast<unsigned int >(seed)};
      auto ud = std::uniform_int_distribution(1,2);
      int guess = ud(mtgen);

      std::cout << "scissor (0), rock (1), paper (2): ";
      int ans{};
      std::cin >> ans;

      if (ans == 1 && guess == 0) {
          std::cout << "The computer is scissor. You are rock. You won\n";
        }
      if (ans == 1 && guess == 1) {
          std::cout << "The computer is rock. You are rock too. It is a draw\n";
        }
      if (ans == 0 && guess == 2){
          std::cout << "The computer is paper. You are scissor. You won\n";
        }
      if(ans == 0 && guess == 0) {
          std::cout << "The computer is scissor. You are scissor too. It is a draw.\n";
        }
      if (ans == 2 && guess == 1) {
          std::cout << "The computer is rock. You are paper. You win\n.";
        }
      if (ans == 2 && guess == 2) {
          std::cout << "The computer is paper. You are paper. It is a draw\n";
        }
    }

    double exe_3_18(int x1, int y1, int x2, int y2, int x3, int y3)
    {
      if (x1 + y1 > x3 && x3 + y3 > x2 + y2 ||
          (x1 + y1 > x2 + y2 && x3 + y3 > x2 + y2) ||
          (x3 + y3 > x1 + y1 && x2 + y2 > x1 + y1)) {
          std::cout << "The input is valid \n";
        } else {
          std::cout << "The inputs are invalid!\n";
          std::exit(0);
        }
      double perimeter{0.0};
      double l1 = sqrt(pow((x2-x1), 2) + pow(y2-y1, 2));
      double l2 = sqrt(pow(x3 - x2, 2) + pow(y3 -y2, 2));
      double l3 = sqrt(pow(x3 - x1, 2) + pow(y3 -y1, 2));
      std::cout << " The perimeter is: " << l1 + l2 + l3;
      perimeter = l1 + l2 + l3;
      return std::round(perimeter * 1000) / 1000.0;
    }

    void exe_3_20()
    {
      // Prompt the user to enter a temperature and a wind speed
      std::cout << "Enter the temperature in Fahrenheit "
                   "between -58F and 41F: ";
      double temperature{0.0};
      std::cin >> temperature;
      std::cout << "Enter the wind speed (>= 2) in miles per hour: ";
      double speed{0.0};
      std::cin >> speed;

      if (temperature <= -58 || temperature >= 41 || speed < 2)
        {
          std::cout << "The ";
          if (temperature <= -58 || temperature >= 41)
            std::cout << "temperature ";
          if ((temperature <= -58 || temperature >= 41) && speed < 2)
            std::cout << "and ";
          if (speed < 2)
            std::cout << "wind speed ";
          std::cout << "is invalid";
          std::exit(1);
        }

      // Compute the wind chill index
      double windChill = 35.74 + 0.6215 * temperature -
          35.75 * pow(speed, 0.16) +
          0.4275 * temperature * pow(speed, 0.16);

      // Display result
      std::cout << "The wind chill index is " << windChill;
    }

    int exe_3_21(int k, int m, int q)
    {
      int h{0};
      int j = (k / 100 );
      k = k % 100;

      if (m <= 0 || m > 12) {
          std::cerr << "Month is out of range [ 1 - 12]\n";
          std::exit(EXIT_FAILURE);
        }
      if(q <=0 || q > 31) {
          std::cerr << "Day of the month is out of range[ 1 - 31]\n";
          std::exit(EXIT_FAILURE);
        }

      if (m == 1 || m == 2)
        {
          m = (m == 1) ? 13 : 14;
          k--;
        }

      h = ( q + ( (26 * (m + 1)) / 10 ) + k +  (k / 4) + (j / 4) + 5 * j ) % 7;
      return h;
    }

    bool exe_3_22(double x, double y)
    {
      double d{ sqrt(pow(x - 0.0, 2) + pow(y - 0.0, 2) ) };
      if ( d <= 10.0)
        return true; /// the point is in the circle
      return false;
    }

    bool exe_3_23(double x, double y)
    {
      if (sqrt(pow(x, 2) + pow(y, 2)) <= (10 / 5) || sqrt(pow(y, 2)) <=(5.0 / 2) )
        return true;
      return false;
    }

    void exe_3_24()
    {
      long seed = std::chrono::system_clock::now().time_since_epoch().count();
      auto mtgen = std::mt19937{ static_cast<unsigned int>(seed) };
      auto ud = std::uniform_int_distribution(1, 13);
      int rank = ud(mtgen);
      auto ud_1_4 = std::uniform_int_distribution(1, 4);
      int suit = ud_1_4(mtgen);

      std::cout << "The card picked from 52 card deck ";
      switch (rank) {
        case 1:
          std::cout << "Ace";
          break;
        case 2:
          std::cout << rank;
          break;
        case 3:
          std::cout << rank;
          break;
        case 4:
          std::cout << rank;
          break;
        case 5:
          std::cout << rank;
          break;
        case 6:
          std::cout << rank;
          break;
        case 7:
          std::cout << rank;
          break;
        case 8:
          std::cout << rank;
          break;
        case 9:
          std::cout << rank;
          break;
        case 10:
          std::cout << rank;
          break;
        case 11:
          std::cout << "Jack";
          break;
        case 12:
          std::cout << "Queen";
          break;
        case 13:
          std::cout << "King";
          break;

        }

      std::cout << " of ";
      switch(suit) {
        case 0:
          std::cout << "clubs\n";
          break;
        case 1:
          std::cout << "diamonds\n";
          break;
        case 2:
          std::cout << "hearts\n";
          break;
        case 3:
          std::cout << "spades\n";
          break;

        }
    }

    void exe_3_25(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4)
    {
      // Calculate the intersecting point
      // Get a, b, c, d, e, f
      double a = y1 - y2;
      double b = -1 * (x1 - x2);
      double c = y3 - y4;
      double d = -1 * (x3 - x4);
      double e = (y1 - y2) * x1 - (x1 - x2) * y1;
      double f = (y3 - y4) * x3 - (x3 - x4) * y3;

      // Display results
      if (a * d - b * c == 0)
        {
          std::cout << "The two lines are parallel.\n";
        }
      else
        {
          double x = (e * d - b * f) / (a * d - b * c);
          double y = (a * f - e * c) / (a * d - b * c);
          std::cout << "The intersecting point is at (" << x << ", " << y << ") \n";
        }
    }

    void exe_3_26()
    {
      std::cout << "Enter an interger: ";
      int number{};
      std::cin >> number;
      // Determine whether it is divisible by 5 and 6
      // Display results
      std::cout << "Is 10 divisible by 5 and 6? " <<
                   ((number % 5 == 0) && (number % 6 == 0)) << std::endl;
      std::cout << "Is 10 divisible by 5 or 6? " <<
                   ((number % 5 == 0) || (number % 6 == 0)) << std::endl;
      std::cout << "Is 10 divisible by 5 of 6, but not both? " <<
                   ((number % 5 == 0) ^ (number % 6 == 0)) << std::endl;
    }

    void exe_3_27()
    {
      std::cout << "Enter a point's x- and y- coordinates: ";
      double x{}, y{};
      std::cin >> x >> y;
      // Determine whether the point is inside the triangle
      // getting the point of ina line that starts at point

      // Get the intersecting point with the hypotenuse side of the triangle
      // of a line that starts and points (0, 0) and touches the user points
      double intersectx = (-x * (200 * 100)) / (-y * 200 - x * 100);
      double intersecty = (-y * (200 * 100)) / (-y * 200 - x * 100);

      // Display results
      std::cout << "The point " << ((x > intersectx || y > intersecty)
                                    ? "is not " : "is " ) << " in the triangle. " << std::endl;
    }

  }
}


#pragma once

#include <random>
#include <math.h>

#include <dtl/dtl.hpp>

#include <boost/math/distributions/poisson.hpp>

//TODO merge into fpr.hpp

namespace dtl {
namespace bloomfilter {

/// Computes an approximation of the false positive probability for standard Bloom filter.
/// Assuming independence for the probabilities of each bit being set.
static f64
fpr(u64 m,
    u64 n,
    f64 k) {
  return std::pow(1.0 - std::pow(1.0 - (1.0 / m), k * n), k);
}

/// Computes an approximation of the false positive probability for Blocked Bloom filter as
/// defined by Putze et al.
/// Note: The formula of Putze does not take self collisions into account and thus leads
///       to a significant error for small block sizes (i.e., register blocks).
///       Therefore, for small block sizes, the 'self collision' flag should be set to true.
static f64
fpr_blocked(u64 m,
            u64 n,
            f64 k,
            u64 B, /* block size in bits */
            u1 self_collisions = false,
            f64 epsilon = 0.000001) {
  $f64 f = 0;
  $f64 c = (m * 1.0) / n;
  $f64 lambda = B / c;
  boost::math::poisson_distribution<> poisson(lambda);

  $f64 k_act = k;
  if (self_collisions) {
    k_act = B * (1.0 - std::pow(1.0-1.0/B, k_act));
  }

  $f64 d_sum = 0.0;
  $u64 i = 0;
  while ((d_sum + epsilon) < 1.0) {
    auto d = boost::math::pdf(poisson, i);
    d_sum += d;
    f += d * fpr(B, i, k_act);
    i++;
  }
  return f;
}

/// Computes an approximation of the false positive probability for
/// Sectorized Blocked Bloom filter.
static f64
fpr_blocked_sectorized(u64 m,
                       u64 n,
                       f64 k,
                       u64 B, /* block size in bits */
                       u64 S, /* sector size in bits */
                       u1 self_collisions = false,
                       f64 epsilon = 0.000001) {
  $f64 f = 0;
  $f64 c = (m * 1.0) / n;
  $f64 lambda = (B * 1.0) / c;
  $f64 s = (B * 1.0) / S;
  boost::math::poisson_distribution<> poisson(lambda);

  $f64 d_sum = 0.0;
  $u64 i = 0;
  $f64 k_per_s = (k * 1.0)/s;
  if (self_collisions) {
    k_per_s = S * (1.0 - std::pow(1.0-1.0/S, k_per_s));
  }
  while ((d_sum + epsilon) < 1.0) {
    auto d = boost::math::pdf(poisson, i);
    d_sum += d;
    f += d * std::pow(fpr(S, i, k_per_s), s);
    i++;
  }
  return f;
}

static __forceinline__ f64
p_load(f64 v, u64 i) {
  f64 lambda = v;
  boost::math::poisson_distribution<> poisson(lambda);
  return boost::math::pdf(poisson, i);
}

static __forceinline__ f64
p_cache(u64 s, u64 S, u64 B, f64 k, u64 i) {
//  boost::math::binomial_distribution binomial();

  f64 k_per_s = (k * 1.0)/s;
  $f64 sum = 0.0;
  for (std::size_t j = 1; j <= i; j++) {
    auto ev = (i*s*S*1.0)/B;
    auto p_l = p_load(ev, j);
    auto f_mini = fpr(S, j, k_per_s);
    sum += p_l * f_mini;
  }
  auto r = std::pow(sum, s);
  return r;
}

/// Computes an approximation of the false positive probability for
/// Sectorized Blocked Bloom filter.
static f64
fpr_zoned(u64 m,
          u64 n,
          f64 k,
          u64 B, /* block size in bits */
          u64 S, /* sector size in bits */
          f64 z, /* the number of zones */
          u1 self_collisions = false,
          f64 epsilon = 0.000001) {
  $f64 f = 0;
  $f64 c = (m * 1.0) / n;
  $f64 lambda = (B * 1.0) / c;
  $f64 s = (B * 1.0) / S;
  boost::math::poisson_distribution<> poisson(lambda);

  $f64 d_sum = 0.0;
  $u64 i = 0;
  $f64 k_per_s = (k * 1.0) / s;
  if (self_collisions) {
    k_per_s = S * (1.0 - std::pow(1.0 - 1.0 / S, k_per_s));
  }
  while ((d_sum + epsilon) < 1.0) {
    auto d = boost::math::pdf(poisson, i);
    d_sum += d;
    f += d * p_cache(z, S, B, k, i);
    i++;
  }
  return f;
}

static __forceinline__ f64
_p_cache(u64 s, u64 S, u64 B, f64 k, u64 i) {
  f64 k_per_s = (k * 1.0)/s;
  $f64 sum = 0.0;
  for (std::size_t j = 1; j <= i; j++) {
    auto ev = (i*s*S*1.0)/B;
    auto p_l = p_load(ev, j);
    auto f_mini = fpr(S, j, k_per_s);
    sum += p_l * f_mini;
  }
  auto r = std::pow(sum, s);
  return r;
}

static f64
fpr_blocked_sectorized_zoned(u64 m,
                             u64 n,
                             u64 k,
                             u64 z, /* the number of zones */
                             u64 B, /* block size in bits */
                             u64 S, /* sector size in bits */
                             u1 self_collisions = false,
                             f64 epsilon = 0.000001) {
  $f64 f = 0;
//  f64 c = (m * 1.0) / n;
//  f64 v = (B * 1.0) / c; // aka 'Poisson lambda'
  f64 s = z;

  $f64 d_sum = 0.0;
  $u64 i = 0;
  $f64 k_per_s = (k * 1.0)/s;
  if (self_collisions) {
    k_per_s = S * (1.0 - std::pow(1.0-1.0/S, k_per_s));
  }
  while ((d_sum + epsilon) < 1.0) {
    auto ev = (n*B*1.0)/m;
    auto d = p_load(ev, i);
    d_sum += d;
    f += d * p_cache(z, S, B, k, i);
    i++;
  }
  return f;
}

} // namespace filter

namespace cuckoofilter {

//// TODO
static f64
fpr(u64 associativity,
    u64 tag_bitlength,
    f64 load_factor) {
//  return (2.0 /*k=2*/ * associativity * load_factor) / (std::pow(2, tag_bitlength) - 1); // no duplicates
  return 1 - std::pow(1.0 - 1 / (std::pow(2.0, tag_bitlength) - 1), 2.0 * associativity * load_factor); // counting - with duplicates
//  return 1 - std::pow(1.0 - 1 / (std::pow(2.0, tag_bitlength)), 2.0 * associativity * load_factor); // counting - with duplicates
}

} // namespace cuckoofilter
} // namespace dtl

//f64
//fpr_k_partitioned(u64 m,
//                  u64 n,
//                  u64 k) {
//  f64 c = (m * 1.0) / n;
//  return fpr((m * 1.0)/k, n, 1);
//}
//f64
//fpr_k_partitioned(u64 m,
//                  u64 n,
//                  u64 k) {
//  f64 c = (m * 1.0) / n;
//  return std::pow(1.0 - std::exp(-(k*1.0) / c), k);
//}

//f64
//fpr_zoned(u64 m,
//          u64 n,
//          u64 k,
//          u64 B, /* block size in bits */
//          u64 z, /* number of zones */
//          f64 epsilon = 0.000001) {
//  return std::pow(fpr_blocked(m/z,n,k/z,64), z);
//}

//f64
//fpr_blocked_k_partitioned(u64 m,
//                          u64 n,
//                          u64 k,
//                          u64 B, /* block size in bits */
//                          f64 epsilon = 0.000001) {
//  $f64 f = 0;
//  $f64 c = (m * 1.0) / n;
//  $f64 lambda = (B * 1.0) / c;
//  boost::math::poisson_distribution<> poisson(lambda);
//
//  std::random_device rd;
//  std::mt19937 gen(rd());
//
//  $f64 d_sum = 0.0;
//  $u64 i = 0;
//  while ((d_sum + epsilon) < 1.0) {
//    auto d = boost::math::pdf(poisson, i);
//    d_sum += d;
//    f += d * fpr_k_partitioned(B, i, k);
//    i++;
//  }
//  return f;
//}

//f64
//fpr_blocked_sectorized(u64 m,
//                       u64 n,
//                       u64 k,
//                       u64 B, /* block size in bits */
//                       u64 S, /* sector size in bits */
//                       f64 epsilon = 0.000001) {
//  $f64 f = 0;
//  $f64 c = (m * 1.0) / n;
//  $f64 lambda = (B * 1.0) / c;
//  $f64 s = (B * 1.0) / S;
//  boost::math::poisson_distribution<> poisson(lambda);
//
//  std::random_device rd;
//  std::mt19937 gen(rd());
//
//  $f64 d_sum = 0.0;
//  $u64 i = 0;
//  while ((d_sum + epsilon) < 1.0) {
//    auto d = boost::math::pdf(poisson, i);
//    d_sum += d;
//    f += d * std::pow(fpr(S, i, (k * 1.0)/s), s);
//    i++;
//  }
//  return f;
//}


#include "numeric_tools.h"

#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/constants/constants.hpp>
#include <unsupported/Eigen/Polynomials>
#include <iostream>

namespace Numerics
{
	double bickley3f(double x)
	{
	   auto f = [&x](double t) {return exp(-x / sin(t)) * sin(t) * sin(t);};
	   boost::math::quadrature::tanh_sinh<double> integrator;
	   return integrator.integrate(f, 0.0, boost::math::constants::half_pi<double>());
	}

	double delk(int a, int b)
	{
		return a == b ? 1.0 : 0.0;
	}

	void diagonalDominanceCheck(Eigen::MatrixXd &matrix)
	{
		Eigen::VectorXd rowSum = Eigen::VectorXd::Zero(matrix.rows());

		bool isWarningPrinted = false;

		for(unsigned i = 0; i < rowSum.size(); i++)
		{
			for(unsigned j = 0; j < rowSum.size(); j++)
		    {
			    rowSum(i) += fabs(matrix(i, j));
			}
		}

		for(unsigned i = 0; i < rowSum.size(); i++)
		{
			if((is_lower(matrix(i, i), (rowSum(i) - matrix(i, i)))) && !isWarningPrinted)
			{
				isWarningPrinted = true;
				out.print(TraceLevel::CRITICAL, "The convergence is not guaranteed, the matrix is not strictly diagonally dominant");
			}
		}
	}

	double bickley3f_old(const double x)
	{
		double intValue = 0.0;
		int N = 200;

    	if(is_lower_equal(x, 0.0))
		{
			intValue = M_PI / 4.0;
		}
		else
		{
			for(int i = -N; i < N; i++)
			{
				double step = 1.0 / 100.0;
				double abscissa = 1.0 - 0.25 * pow((tanh(0.5 * M_PI * sinh(i * step)) + 1.0), 2);
				double func = exp(-x / sqrt(abscissa)) * sqrt(abscissa);
				double weight = 0.5 * step * M_PI * cosh(i * step) / pow(cosh(0.5 * M_PI * sinh(i * step)), 2);
				intValue = weight * func / 2.0 + intValue;
			}
		}
		return intValue;
	}

	// https://www.geeksforgeeks.org/multiply-two-polynomials-2/
	std::vector<double> multiply_poly(std::vector<double> &a,
	                                  std::vector<double> &b)
	{
	   size_t m = a.size();
	   size_t n = b.size();

	   std::vector<double> result(m + n - 1, 0.0);

	   for (size_t i = 0; i < m; i++)
	     for (size_t j = 0; j < n; j++)
	           result[i + j] += a[i] * b[j];

	   return result;
	}

	// product of (a1 + x)(a2 + x)(a3 + x)...
	std::vector<double> prod_poly(std::vector<double> &a)
	{
	    std::vector<double> result = {1.0};

		for(auto & a_i : a)
		{
	        std::vector<double> temp = {a_i, 1.0};
			result = multiply_poly(result, temp);
		}

		return result;
	}

	std::vector<double> prod_poly_i(std::vector<double> &a, size_t i)
	{
	    std::vector<double> result = {1.0};

		for(size_t j = 0; j < a.size(); j++)
		{
			if (j == i) continue;
	        std::vector<double> temp = {a[j], 1.0};
			result = multiply_poly(result, temp);
		}

		return result;
	}

	// http://www.ce.unipr.it/people/medici/eigen-poly.html
	std::vector<double> poly_roots(std::vector<double> &p)
	{
		std::vector<double> result;

		Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
		Eigen::VectorXd coeff = Eigen::VectorXd::Zero(p.size());

		for(size_t i = 0; i < p.size(); i++)
	    	coeff[i] = p[i];

		solver.compute(coeff);

		const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();

		for(unsigned i = 0; i < r.rows(); i++)
			result.push_back(r[i].real());

		return result;
	}

	Eigen::VectorXd tridiag_solver(const Eigen::VectorXd &a,
	                               const Eigen::VectorXd &b,
	              	               const Eigen::VectorXd &c,
								   const Eigen::VectorXd &d)
	{
	   int n = d.size();
	   Eigen::VectorXd result = Eigen::VectorXd::Zero(n);
	   Eigen::VectorXd P = Eigen::VectorXd::Zero(n);
	   Eigen::VectorXd Q = Eigen::VectorXd::Zero(n);
	   result = P;

	   // Forward pass
	   P(0) = -c(0) / b(0);
	   Q(0) =  d(0) / b(0);

	   for (int i = 1; i < n; i++)
	   {
	      double denominator = b(i) + a(i - 1) * P(i - 1);
	      P(i) = -c(i - 1)                    / denominator;
	      Q(i) = (d(i) - a(i - 1) * Q(i - 1)) / denominator;
	   }

	   // Backward pass
	   result(n - 1) = Q(n - 1);
	   for (int i = n - 2; i >= 0; i--)
	      result(i) = P(i) * result(i + 1) + Q(i);

	    return result;
	}

	Eigen::VectorXd ConcatenateEigenVectors(Eigen::VectorXd a, Eigen::VectorXd b)
	{
		Eigen::VectorXd result(a.size() + b.size());
		result << a, b;
		return result;
	}

	SourceIterResults sourceIteration(Eigen::MatrixXd &Mmatrix, Eigen::MatrixXd &Fmatrix,
                                      SolverData &solverData, Eigen::VectorXd volumes)
	{
		diagonalDominanceCheck(Mmatrix);

		if(Mmatrix.size() != Fmatrix.size())
		{
			out.print(TraceLevel::CRITICAL, " MMatrix has a different number of elements than FMatrix!");
			exit(-1);
		}

		unsigned size = sqrt(Mmatrix.size());

		Eigen::VectorXd source1      = Eigen::VectorXd::Zero(size);
		Eigen::VectorXd source2      = Eigen::VectorXd::Ones(size);
		Eigen::VectorXd neutronFlux1 = Eigen::VectorXd::Ones(size);
		Eigen::VectorXd neutronFlux2 = Eigen::VectorXd::Zero(size);

		Eigen::VectorXd volumesVec = volumes;

		int energyGroups = double(size) / volumes.size();

		for(auto i = 0; i < energyGroups - 1; i++)
			volumesVec = ConcatenateEigenVectors(volumesVec, volumes);

		double kFactor1 = 1.0;
		double kFactor2 = 0.0;

		int max_iter_number = solverData.getMaxIterNumber();
		double accuracy = solverData.getAccuracy();
		std::string title = get_name(solverData.getKind());

		int h;

		Eigen::ColPivHouseholderQR<Eigen::MatrixXd> CPHQR;
		CPHQR.compute(Mmatrix);

		for(h = 0; h < max_iter_number; h++)
		{
			neutronFlux2 = CPHQR.solve(source2);

			source1 = Fmatrix * neutronFlux1;
			source2 = Fmatrix * neutronFlux2;

			double sum1 = std::inner_product(source1.begin(), source1.end(), source2.begin(), 0.0);
			double sum2 = std::inner_product(source2.begin(), source2.end(), source2.begin(), 0.0);

			kFactor2 = kFactor1 * (sum2 / sum1);

			if(solverData.getKind() == SolverKind::DIFFUSION)
			{
				source2 = source2.cwiseProduct(volumesVec);
			}

			source2 /= kFactor2;

			// exit condition
			if (fabs((kFactor2 - kFactor1) / kFactor2) < accuracy) break;

			kFactor1     = kFactor2;
			neutronFlux1 = neutronFlux2;
		}

		out.print(TraceLevel::DEBUG, "Number of {} iteration: {}", title, h + 1);

		if(h + 1 > max_iter_number)
		{
			out.print(TraceLevel::CRITICAL, "Number of {} iteration: {}", title, h + 1);
			out.print(TraceLevel::CRITICAL, "The {} calculation did not converge!", title);
			exit(-1);
		}

		Eigen::VectorXd neutronFlux = neutronFlux2 / neutronFlux2.sum();
		double kFactor = kFactor2;

		SourceIterResults result(neutronFlux, kFactor);
		return result;
	}
}


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

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/2, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}


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

using namespace Eigen;

int main()
{
  Matrix2d a;
  a << 1, 2,
       3, 4;
  Vector3d v(1,2,3);
  std::cout << "a * 2.5 =\n" << a * 2.5 << std::endl;
  std::cout << "0.1 * v =\n" << 0.1 * v << std::endl;
  std::cout << "Doing v *= 2;" << std::endl;
  v *= 2;
  std::cout << "Now v =\n" << v << std::endl;
}


#include "ptEngine.hpp"
#include <Eigen/Dense>

namespace tapl {
	namespace pte {
		// constructor
		template <typename PointT>
		Line<PointT>::Line() {}

		// de-constructor
		template <typename PointT>
		Line<PointT>::~Line() {}

		template <typename PointT>
		std::vector<float> Line<PointT>::fitSVD(std::vector<float> &x, std::vector<float> &y)
		{
			/*
			System of linear equations of the form Ax = 0

			SVD method solves the equation Ax = 0 by performing
			singular-value decomposition of matrix A
			*/

			// Form Matrix A
			Eigen::MatrixXd A(x.size(), 3);
			for(int i = 0; i < x.size(); ++i)
			{
				A(i, 0) = x[i];
				A(i, 1) = y[i];
				A(i, 2) = 1.0;
			}

			// Take SVD of A
			Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::DecompositionOptions::ComputeThinU | Eigen::DecompositionOptions::ComputeThinV);
			Eigen::MatrixXd V = svd.matrixV();

			// store in vector
			std::vector<float> line_coeffs;
			for(auto i = 0; i < V.rows(); ++i)
			{
				line_coeffs.push_back(V(i, (V.cols() - 1)));
			}

			// Return coeffs
			return line_coeffs;
		}

		template <typename PointT>
		std::vector<float> Line<PointT>::fitLS(std::vector<float> &x, std::vector<float> &y)
		{
			/*
			System of linear equations of the form Y = Hx

			least-squares method attempts to minimize
			the energy of error, J(x) = ( ||Y - Hx|| )^2
			where, ||Y - Hx|| is the Euclidian length of vector (Y - Hx)
			*/

			// Form Matrix H
			Eigen::MatrixXd H(x.size(), 2);
			for(int i = 0; i < x.size(); ++i)
			{
				H(i, 0) = x[i];
				H(i, 1) = 1.0;
			}

			// Form Matrix Y
			Eigen::MatrixXd Y(y.size(), 1);
			for(int i = 0; i < y.size(); ++i)
			{
				Y(i, 0) = y[i];
			}

			// Transpose of H
			auto H_transpose = H.transpose();

			// get line coefficients
			auto coeffs = ((H_transpose*H).inverse()) * (H_transpose*Y);

			// Line equation is of the form y = a'x + b'
			// let's convert it to the form ax + by + c = 0
			// a = a'; b = -1; c = b'
			std::vector<float> line_coeffs;
			line_coeffs.push_back(coeffs(0, 0));
			line_coeffs.push_back(-1.0);
			line_coeffs.push_back(coeffs(1, 0));

			// Return coefficients
			return line_coeffs;
		}

		template <typename PointT>
		float Line<PointT>::distToPoint(std::vector<float> line_coeffs, PointT point)
		{
			float dist = fabs(line_coeffs[0] * point.x + line_coeffs[1] * point.y + line_coeffs[2]) /
							sqrt(pow(line_coeffs[0], 2) + pow(line_coeffs[1], 2));

			return dist;
		}

		template <typename PointT>
		std::unordered_set<int> Line<PointT>::Ransac(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distTolerance)
		{
			// random number seed
			srand(time(NULL));
			// get the start timestamp
			auto t_start = std::chrono::high_resolution_clock::now();

			std::unordered_set<int> inliersResult;

			// number of random samples to select per iteration
			const int n_random_samples = 2;

			// number of inliers for each iteration
			std::vector<int> n_inliers(maxIterations, 0);
			// coefficients for each line
			std::vector<std::vector<float>> coeffs(maxIterations);

			// iterate 'maxIterations' number of times
			for(int i = 0; i < maxIterations; ++i)
			{
				// x, y, and z points as a vector
				std::vector<float> x, y, z;
				// select random samples
				for(int j = 0; j < n_random_samples; ++j)
				{
					int idx = rand()%cloud->size();
					x.push_back(cloud->at(idx).x);
					y.push_back(cloud->at(idx).y);
					z.push_back(cloud->at(idx).z);
				}
				// fit a line
				coeffs[i] = this->fitSVD(x, y);

				for(typename pcl::PointCloud<PointT>::iterator it = cloud->begin(); it != cloud->end(); ++it)
				{
					if(this->distToPoint(coeffs[i], *it) <= distTolerance)
					{
						n_inliers[i]++;
					}
				}
			}

			// find the index for number of inliers
			auto inliers_it = std::max_element(n_inliers.begin(), n_inliers.end());
			int index_max_n_inlier = std::distance(n_inliers.begin(), inliers_it);

			// find inliers with the best fit
			int index = 0;
			for(typename pcl::PointCloud<PointT>::iterator it = cloud->begin(); it != cloud->end(); ++it)
			{
				if(this->distToPoint(coeffs[index_max_n_inlier], *it) <= distTolerance)
				{
					inliersResult.insert(index);
				}
				index++;
			}

			// get the end timestamp
			auto t_end = std::chrono::high_resolution_clock::now();

			// measure execution time
			auto t_duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start);
			TLOG_INFO << "Time taken by RANSAC: "
				<< t_duration.count() << " milliseconds" ;

			// Return indicies of inliers from fitted line with most inliers
			return inliersResult;
		}

		// constructor
		template <typename PointT>
		Plane<PointT>::Plane() {}

		// de-constructor
		template <typename PointT>
		Plane<PointT>::~Plane() {}

		template <typename PointT>
		std::vector<float> Plane<PointT>::fitSVD(std::vector<float> &x, std::vector<float> &y, std::vector<float> &z)
		{
			/*
			System of linear equations of the form Ax = 0

			SVD method solves the equation Ax = 0 by performing
			singular-value decomposition of matrix A
			*/

			// Form Matrix A
			Eigen::MatrixXd A(x.size(), 4);
			for(int i = 0; i < x.size(); ++i)
			{
				A(i, 0) = x[i];
				A(i, 1) = y[i];
				A(i, 2) = z[i];
				A(i, 3) = 1.0;
			}

			// Take SVD of A
			Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::DecompositionOptions::ComputeThinU | Eigen::DecompositionOptions::ComputeThinV);
			Eigen::MatrixXd V = svd.matrixV();

			// Plane equation is of the form ax + by + cz + d = 0
			std::vector<float> plane_coeffs;
			for(auto i = 0; i < V.rows(); ++i)
			{
				plane_coeffs.push_back(V(i, (V.cols() - 1)));
			}

			// Return coeffs
			return plane_coeffs;
		}

		template <typename PointT>
		std::vector<float> Plane<PointT>::fitLS(std::vector<float> &x, std::vector<float> &y, std::vector<float> &z)
		{
			/*
			System of linear equations of the form Y = Hx

			least-squares method attempts to minimize
			the energy of error, J(x) = ( ||Y - Hx|| )^2
			where, ||Y - Hx|| is the Euclidian length of vector (Y - Hx)
			*/

			// Form Matrix H
			Eigen::MatrixXd H(x.size(), 3);
			for(int i = 0; i < x.size(); ++i)
			{
				H(i, 0) = x[i];
				H(i, 1) = y[i];
				H(i, 2) = 1.0;
			}

			// Form Matrix Y
			Eigen::MatrixXd Y(z.size(), 1);
			for(int i = 0; i < z.size(); ++i)
			{
				Y(i, 0) = z[i];
			}

			// Transpose of H
			auto H_transpose = H.transpose();

			// get plane coefficients
			auto coeffs = ((H_transpose*H).inverse()) * (H_transpose*Y);

			// Plane equation is of the form z = a'x + b'y + c'
			// let's convert it to the form ax + by + cz + d = 0
			// a = a'; b = b'; c = -1; d = c'
			std::vector<float> plane_coeffs;
			plane_coeffs.push_back(coeffs(0, 0));
			plane_coeffs.push_back(coeffs(1, 0));
			plane_coeffs.push_back(-1.0);
			plane_coeffs.push_back(coeffs(2, 0));

			// Return coefficients
			return plane_coeffs;
		}

		template <typename PointT>
		float Plane<PointT>::distToPoint(std::vector<float> plane_coeffs, PointT point)
		{
			float dist = fabs(plane_coeffs[0] * point.x + plane_coeffs[1] * point.y + plane_coeffs[2] * point.z + plane_coeffs[3]) /
							sqrt(pow(plane_coeffs[0], 2) + pow(plane_coeffs[1], 2) + pow(plane_coeffs[2], 2));

			return dist;
		}

		template <typename PointT>
		std::unordered_set<int> Plane<PointT>::Ransac(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceToPlane)
		{
			// random number seed
			srand(time(NULL));
			// get the start timestamp
			auto t_start = std::chrono::high_resolution_clock::now();

			std::unordered_set<int> inliersResult;

			// number of random samples to select per iteration
			const int n_random_samples = 3;

			// number of inliers for each iteration
			std::vector<int> n_inliers(maxIterations, 0);
			// coefficients for each plane
			std::vector<std::vector<float>> coeffs(maxIterations);

			// iterate 'maxIterations' number of times
			for(int i = 0; i < maxIterations; ++i)
			{
				// x, y, and z points as a vector
				std::vector<float> x, y, z;
				// select random samples
				for(int j = 0; j < n_random_samples; ++j)
				{
					int idx = rand()%(cloud->size());
					x.push_back(cloud->at(idx).x);
					y.push_back(cloud->at(idx).y);
					z.push_back(cloud->at(idx).z);
				}
				// fit a plane
				coeffs[i] = this->fitLS(x, y, z);

				for(typename pcl::PointCloud<PointT>::iterator it = cloud->begin(); it != cloud->end(); ++it)
				{
					// TLOG_INFO << "dist = " << this->distToPoint(coeffs[i], *it) ;
					if(this->distToPoint(coeffs[i], *it) <= distanceToPlane)
					{
						n_inliers[i]++;
					}
				}
			}

			// find the index for number of inliers
			auto inliers_it = std::max_element(n_inliers.begin(), n_inliers.end());
			int index_max_n_inlier = std::distance(n_inliers.begin(), inliers_it);

			// find inliers with the best fit
			int index = 0;
			for(typename pcl::PointCloud<PointT>::iterator it = cloud->begin(); it != cloud->end(); ++it)
			{
				if(this->distToPoint(coeffs[index_max_n_inlier], *it) <= distanceToPlane)
				{
					inliersResult.insert(index);
				}
				index++;
			}

			// get the end timestamp
			auto t_end = std::chrono::high_resolution_clock::now();

			// measure execution time
			auto t_duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start);

			// Return indicies of inliers from fitted line with most inliers
			return inliersResult;
		}

		void KdTree::insertHelper(Node ** node, unsigned depth, std::vector<float> point, int id)
		{
			if(*node != NULL) {
				// x split when (depth % 3) = 0; y split when (depth % 3) = 1; z split when (depth % 3) = 2
				// index for accessing point.x is 0; index for accessing point.y is 1; index for accessing point.z is 2
				if(point[(depth % 3)] < ((*node)->point[(depth % 3)])) {
					node = &((*node)->left);
				}
				else {
					node = &((*node)->right);
				}

				// call this function recursively until a NULL is hit
				insertHelper(node, depth+1, point, id);
			}
			else {
				// create a node and insert the point
				*node = new Node(point, id);
			}
		}

		void KdTree::insert(std::vector<float> point, int id)
		{
			// This function inserts a new point into the tree
			// the function creates a new node and places correctly with in the root
			insertHelper(&this->root, 0, point, id);
		}

		// this function returns euclidian distance between two points
		float KdTree::dist(std::vector<float> point_a, std::vector<float> point_b)
		{
			// compute distance
			float dist = sqrt(pow((point_a[0] - point_b[0]), 2)
							+ pow((point_a[1] - point_b[1]), 2)
							+ pow((point_a[2] - point_b[2]), 2));

			// Return the euclidian distance between points
			return dist;
		}

		void KdTree::searchHelper(Node * node, std::vector<float> target, float distTolerance, int depth, std::vector<int>& ids)
		{
			if(node != NULL) {
				// add this node id to the list if its distance from target is less than distTolerance
				if(dist(node->point, target) <= distTolerance)
					ids.push_back(node->id);

				// x split when (depth % 3) = 0; y split when (depth % 3) = 1; z split when (depth % 3) = 2
				// index for accessing point.x is 0; index for accessing point.y is 1; index for accessing point.z is 2
				if((target[depth % 3] - distTolerance) < node->point[(depth % 3)])
					searchHelper(node->left, target, distTolerance, depth+1, ids);
				if((target[depth % 3] + distTolerance) > node->point[(depth % 3)])
					searchHelper(node->right, target, distTolerance, depth+1, ids);
			}
		}
		// return a list of point ids in the tree that are within distance of target
		std::vector<int> KdTree::search(std::vector<float> target, float distTolerance)
		{
			std::vector<int> ids;
			searchHelper(this->root, target, distTolerance, 0, ids);
			return ids;
		}

		void EuclideanCluster::proximityPoints( int pointIndex,
								std::vector<bool>& checked,
								float distTolerance,
								std::vector<int>& cluster)
		{
			std::vector<int> nearby = this->tree->search(this->points[pointIndex], distTolerance);
			for(auto it = nearby.begin(); it != nearby.end(); ++it) {
				if(! checked[*it]) {
					checked[*it] = true;
					cluster.push_back(*it);
					// call this function recursively to find all the points within proximity (i.e. points within proximity of proximity)
					proximityPoints(*it, checked, distTolerance, cluster);
				}
			}
		}

		std::vector<std::vector<int>> EuclideanCluster::clustering(float distTolerance)
		{
			std::vector<std::vector<int>> clusters;

			// vector to keep track of checked points
			std::vector<bool> checked(points.size(), false);
			for(int i = 0; i < this->points.size(); ++i) {
				// create a new cluster if this point was not processed already
				if(! checked[i]) {
					std::vector<int> cluster;
					checked[i] = true;
					cluster.push_back(i);
					// find points within the proximity
					proximityPoints(i, checked, distTolerance, cluster);
					// add this cluster to the vector of clusters
					clusters.push_back(cluster);
				}
			}

			return clusters;
		}

		/**
         * @brief returns world to camera rotation matrix
         *
         *   Camera Coordinate System:
         *       X -> To the right
         *       Y -> Down
         *       Z -> Forward - Direction where the camera is pointing
         *
         *   World Coordinate System:
         *       X -> Forward - Direction where the camera is pointing
         *       Y -> To the left
         *       Z -> Up
         * @return rotation matrix
         */
        cv::Mat world2CamRotation() {
            // camera coordinate to world coordinate rotation matrix
            cv::Mat R = cv::Mat::zeros(3, 3, CV_32F);
            // Camera rotation
            float Rx = degreesToRadians(-90);
            float Ry = degreesToRadians(0);
            float Rz = degreesToRadians(-90);

            // Rz
            cv::Mat R_z = cv::Mat::eye(3, 3, CV_32F);
            R_z.at<float>(0, 0) = cos(Rz);
            R_z.at<float>(0, 1) = -sin(Rz);
            R_z.at<float>(1, 0) = sin(Rz);
            R_z.at<float>(1, 1) = cos(Rz);
            // Ry
            cv::Mat R_y = cv::Mat::eye(3, 3, CV_32F);
            R_y.at<float>(0, 0) = cos(Ry);
            R_y.at<float>(0, 2) = sin(Ry);
            R_y.at<float>(2, 0) = -sin(Ry);
            R_y.at<float>(2, 2) = cos(Ry);
            // Rx
            cv::Mat R_x = cv::Mat::eye(3, 3, CV_32F);
            R_y.at<float>(1, 1) = cos(Rx);
            R_y.at<float>(1, 2) = -sin(Rx);
            R_y.at<float>(2, 1) = sin(Rx);
            R_y.at<float>(2, 2) = cos(Rx);

            // Camera Rotation Correction Matrix
            R = R_z * R_y * R_x;

			// return rotation matrix
            return R;
        }

        /**
         * @brief affine transform on a point
         *
         * Apply affine transforms on point given in world coordinate
         *
         *
         *   Camera Coordinate System:
         *       X -> To the right
         *       Y -> Down
         *       Z -> Forward - Direction where the camera is pointing
         *
         *   World Coordinate System:
         *       X -> Forward - Direction where the camera is pointing
         *       Y -> To the left
         *       Z -> Up
         *
         * @param[in] point point in world coordinate
         *
         * @return point in camera coordinate
         */
        template <typename PointT>
        void world2CamCoordinate(PointT &point) {
            // Camera Rotation Correction Matrix
            cv::Mat R = world2CamRotation();

            cv::Mat xyz = cv::Mat(3, 1, CV_32F);
            xyz.at<float>(0, 0) = point.x;
            xyz.at<float>(1, 0) = point.y;
            xyz.at<float>(2, 0) = point.z;

            cv::Mat xyz_w = cv::Mat(3, 1, CV_32F);

            xyz_w = R * xyz;
            point.x = xyz_w.at<float>(0, 0);
            point.y = xyz_w.at<float>(1, 0);
            point.z = xyz_w.at<float>(2, 0);
        }
	}
}

// explicit instantiation to avoid linker error
template class tapl::pte::Line<tapl::Point3d>;
template class tapl::pte::Line<pcl::PointXYZ>;
template class tapl::pte::Line<pcl::PointXYZI>;
template class tapl::pte::Line<pcl::PointXYZRGB>;
template class tapl::pte::Plane<tapl::Point3d>;
template class tapl::pte::Plane<pcl::PointXYZ>;
template class tapl::pte::Plane<pcl::PointXYZI>;
template class tapl::pte::Plane<pcl::PointXYZRGB>;