// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_COMPUTEROOTS_HPP
#define MCL_COMPUTEROOTS_HPP 1

#include <Eigen/Core>
//#include <cmath>
#include <complex>

namespace mcl
{

// Returns the smallest, positive, real root for quadratic
// a x^2 + b x + c
// Returns a negative value if no roots found
// from https://github.com/mike323zyf/BCQN
static inline double compute_quad_roots(double a, double b, double c, double tol=1e-16)
{
	double t = -1;
	if( std::abs(a) <= tol ){ t = -c / b; }
	else {
		double desc = b*b - 4.0 * a*c;
		if( desc > 0.0 ){
			t = (-b - std::sqrt(desc)) / (2.0 * a);
			if (t < 0.0){
				t = (-b + std::sqrt(desc)) / (2.0 * a);
			}
		}
		else{ // linear
			t = -b / (2.0*a);
		}
	}
	return t;
}

// Returns a negative value if no roots found
// from: https://github.com/libigl/libigl/blob/main/include/igl/flip_avoiding_line_search.cpp
static inline double compute_quad_roots_2(double a, double b, double c)
{
	double t1 = 0, t2 = 0;
	const double polyCoefEps = 1e-16;
	const double max_time = 2;
	if (abs(a) > polyCoefEps)
  {
		double delta_in = pow(b, 2) - 4 * a*c;
		if (delta_in < 0) {
			return -1;
		}
		double delta = sqrt(delta_in);
		t1 = (-b + delta) / (2 * a);
		t2 = (-b - delta) / (2 * a);
	}
	else if (abs(b) > polyCoefEps){
		 t1 = t2 = -c / b;
	} else {
		return -1;
	}
	if (t1 < 0) t1 = max_time;
	if (t2 < 0) t2 = max_time;

	if (!std::isfinite(t1) || !std::isfinite(t2))
		return -1;

	double tmp_n = std::min(t1, t2);
	t1 = std::max(t1, t2); t2 = tmp_n;
	if (t1 > 0) {
		if (t2 > 0) {
			return t2;
		}
		else {
			return t1;
		}
	}
	else {
		return -1;
	}
}

// Returns the smallest, positive, real root for cubic
// a x^3 + b x^2 + c x + d
// Returns a negative value if no roots found
// from https://github.com/mike323zyf/BCQN
static inline double compute_cubic_roots(double a, double b, double c, double d, double tol=1e-16)
{
	double t = -1;
	if(std::abs(a) <= tol){ t = compute_quad_roots(b, c, d, tol); }
	else {
		std::complex<double> i(0, 1);
		std::complex<double> delta0(b*b - 3 * a*c, 0);
		std::complex<double> delta1(2 * b*b*b - 9 * a*b*c + 27 * a*a*d, 0);
		std::complex<double> C = pow((delta1 + sqrt(delta1*delta1 - 4.0 * delta0*delta0*delta0)) / 2.0, 1.0 / 3.0);

		std::complex<double> u2 = (-1.0 + sqrt(3.0)*i) / 2.0;
		std::complex<double> u3 = (-1.0 - sqrt(3.0)*i) / 2.0;

		std::complex<double> t1 = (b + C + delta0 / C) / (-3.0*a);
		std::complex<double> t2 = (b + u2*C + delta0 / (u2*C)) / (-3.0*a);
		std::complex<double> t3 = (b + u3*C + delta0 / (u3*C)) / (-3.0*a);

		if ((std::abs(std::imag(t1))<tol) && (std::real(t1)>0))
			t = std::real(t1);
		if ((std::abs(std::imag(t2))<tol) && (std::real(t2)>0) && ((std::real(t2) < t) || (t < 0)))
			t = std::real(t2);
		if ((std::abs(std::imag(t3))<tol) && (std::real(t3)>0) && ((std::real(t3) < t) || (t < 0)))
			t = std::real(t3);
	}
	return t;
}

// src: https://github.com/libigl/libigl/blob/main/include/igl/flip_avoiding_line_search.cpp
static inline double compute_min_pos_root_2D(const Eigen::MatrixXd& x, const Eigen::MatrixXi& E, Eigen::MatrixXd& p, int f)
{
	int v1 = E(f,0);
	int v2 = E(f,1);
	int v3 = E(f,2);
	const double& U11 = x(v1,0);
	const double& U12 = x(v1,1);
	const double& U21 = x(v2,0);
	const double& U22 = x(v2,1);
	const double& U31 = x(v3,0);
	const double& U32 = x(v3,1);
	const double& V11 = p(v1,0);
	const double& V12 = p(v1,1);
	const double& V21 = p(v2,0);
	const double& V22 = p(v2,1);
	const double& V31 = p(v3,0);
	const double& V32 = p(v3,1);
	double a = V11*V22 - V12*V21 - V11*V32 + V12*V31 + V21*V32 - V22*V31;
	double b = U11*V22 - U12*V21 - U21*V12 + U22*V11 - U11*V32 + U12*V31 + U31*V12 - U32*V11 + U21*V32 - U22*V31 - U31*V22 + U32*V21;
	double c = U11*U22 - U12*U21 - U11*U32 + U12*U31 + U21*U32 - U22*U31;
  double root = compute_quad_roots_2(a,b,c);
  if (root < 0) { return std::numeric_limits<float>::max(); }
  return root;
}

} // end mcl

#endif


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

void testEuclideanNorm()
{
  Eigen::Matrix<double, 3, 5> A;
  A << 1, 2, 3, 4, 5,
       2, 3, 4, 5, 6,
       7, 9, 10, 11, 12;

  std::cout << A.colwise().norm() << std::endl << std::endl;

  Eigen::Matrix<double, 3, 5> B;
  B = A.array().rowwise() / A.colwise().norm().array();
  std::cout << B << std::endl;

  for(int i=0; i<A.cols(); ++i)
  {
    Eigen::Vector3d b = A.col(i)/A.col(i).norm();
    std::cout << b.transpose() << std::endl;
    std::cout << b.norm() << std::endl;
  }

}

int main(int argc, char** argv)
{
  testEuclideanNorm();
  return 0;
}


/*
 * Website:
 *      https://github.com/wo3kie/dojo
 *
 * Author:
 *      Lukasz Czerwinski
 *
 * Compilation:
 *      g++ --std=c++11 interpolation.cpp -o interpolation -lgsl -lgslcblas -lm
 *
 * Usage:
 *      $ ./interpolation
 */

#include <algorithm>
#include <vector>

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

#include <gsl/gsl_linalg.h>

#include "./feq.hpp"

namespace ublas = boost::numeric::ublas;

struct Point
{
    double x_;
    double y_;
};

ublas::vector< double > linear_interpolation_ublas(
    std::vector< Point > const & points
){
    assert( points.size() == 2 );

    ublas::vector< double > X( 2 );

    Point const & p1 = points[ 0 ];
    Point const & p2 = points[ 1 ];

    double const a = (p1.y_ - p2.y_) / (p1.x_ - p2.x_);
    double const b = p1.y_ - p1.x_ * ((p1.y_ - p2.y_) / (p1.x_ - p2.x_));

    X( 0 ) = a;
    X( 1 ) = b;

    return X;
}

ublas::vector< double > polynominal_interpolation_ublas( std::vector< Point > const & points ){
    std::size_t const size = points.size();

    ublas::matrix< double > A( size, size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        for( std::size_t j = 0 ; j < size ; ++ j ){
            A( i, j ) = std::pow( points[ i ].x_, j );
        }
    }

    ublas::vector< double > Y( size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        Y( i ) = points[ i ].y_;
    }

    ublas::permutation_matrix< double > PM( size );
    ublas::lu_factorize( A, PM );
    ublas::lu_substitute( A, PM, Y );

    return Y;
}

gsl_vector * linear_interpolation_gsl( std::vector< Point > const & points ){
    assert( points.size() == 2 );

    gsl_vector * X = gsl_vector_alloc( 2 );

    Point const & p1 = points[ 0 ];
    Point const & p2 = points[ 1 ];

    double const a = (p1.y_ - p2.y_) / (p1.x_ - p2.x_);
    double const b = p1.y_ - p1.x_ * ((p1.y_ - p2.y_) / (p1.x_ - p2.x_));

    gsl_vector_set( X, 0, a );
    gsl_vector_set( X, 1, b );

    return X;
}

gsl_vector * polynominal_interpolation_gsl( std::vector< Point > const & points ){
    std::size_t const size = points.size();

    gsl_matrix * A = gsl_matrix_alloc( size, size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        for( std::size_t j = 0 ; j < size ; ++ j ){
            gsl_matrix_set( A, i, j, std::pow( points[ i ].x_, j ) );
        }
    }

    gsl_vector * Y = gsl_vector_alloc( size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        gsl_vector_set( Y, i, points[ i ].y_ );
    }

    gsl_vector * X = gsl_vector_alloc( size );
    gsl_permutation * PM = gsl_permutation_alloc( size );

    int s = 0;
    gsl_linalg_LU_decomp( A, PM, &s );
    gsl_linalg_LU_solve( A, PM, Y, X );

    gsl_permutation_free( PM );
    gsl_vector_free( Y );
    gsl_matrix_free( A );

    return X;
}

void linear_interpolation_ublas_test(){
    ublas::vector< double > expected( 2 );
    expected( 0 ) = 5.0 / 2;
    expected( 1 ) = -13.0 / 2;

    ublas::vector< double > const actual = linear_interpolation_ublas(
        std::vector< Point >{ { 5, 6 }, { 7, 11 } }
    );

    auto feqDouble = [](double d1, double d2){
        return feq(d1, d2);
    };

    assert(
        std::equal(
            actual.begin(),
            actual.end(),
            expected.begin(),
            feqDouble
        )
    );
}

void polynominal_interpolation_ublas_test(){
    ublas::vector< double > expected( 3 );
    expected( 0 ) = 7;
    expected( 1 ) = 6;
    expected( 2 ) = -1;

    ublas::vector< double > const actual = polynominal_interpolation_ublas(
        std::vector< Point >{ { 1, 12 }, { 2, 15 }, { 3, 16 } }
    );

    auto feqDouble = [](double d1, double d2){
        return feq(d1, d2);
    };

    assert(
        std::equal(
            actual.begin(),
            actual.end(),
            expected.begin(),
            feqDouble
        )
    );
}

void linear_interpolation_gsl_test(){
    gsl_vector * expected = gsl_vector_alloc( 2 );
    gsl_vector_set( expected, 0, 5.0 / 2 );
    gsl_vector_set( expected, 1, -13.0 / 2 );

    gsl_vector * actual = linear_interpolation_gsl(
        std::vector< Point >{ { 5, 6 }, { 7, 11 } }
    );

    for( std::size_t i = 0 ; i < 2 ; ++ i ){
        assert((
            feq(
                gsl_vector_get( actual, i ),
                gsl_vector_get( expected, i )
            )
        ));
    }

    gsl_vector_free( actual );
    gsl_vector_free( expected );
}

void polynominal_interpolation_gsl_test(){
    gsl_vector * expected = gsl_vector_alloc( 3 );
    gsl_vector_set( expected, 0, 7 );
    gsl_vector_set( expected, 1, 6 );
    gsl_vector_set( expected, 2, -1 );

    gsl_vector * actual = polynominal_interpolation_gsl(
        std::vector< Point >{ { 1, 12 }, { 2, 15 }, { 3, 16 } }
    );

    for( std::size_t i = 0 ; i < 3 ; ++ i ){
        assert((
            feq(
                gsl_vector_get( actual, i ),
                gsl_vector_get( expected, i )
            )
        ));
    }

    gsl_vector_free( actual );
    gsl_vector_free( expected );
}

#include <vector>

int main(){
    linear_interpolation_ublas_test();
    linear_interpolation_gsl_test();

    polynominal_interpolation_ublas_test();
    polynominal_interpolation_gsl_test();
}


/*
 * Website:
 *      https://github.com/wo3kie/dojo
 *
 * Author:
 *      Lukasz Czerwinski
 *
 * Compilation:
 *      g++ --std=c++11 bond.cpp -o bond -lboost_date_time
 *
 * Usage:
 *      $ ./bond 2015/12/31 100 0.08 0.08 2014/12/31
 *        PV( F ): 92.5926
 *        PV( Coupon ): 7.40741
 *      P: 100
 */

/*
 *                                                                      Maturity Date
 *                                                                       |
 *                     /-- 1 year -----------    -- 1 year------------\  |
 *                    /                      \  /                      \ |
 *                   v                        \v                        \v
 * ... - 2 - 4 - 6 - 8 - 10 - 12 - 2 - 4 - 6 - 8 - 10 - 12 - 2 - 4 - 6 - 8 - 10 - ...
 *       ^           ^                         ^                         ^
 *       |           |                         |                         |
 *       |          Coupon                    Coupon                    Face Value
 *      Price Day                                                       Coupon
 */

#include <cmath>
#include <iostream>

#include <boost/date_time/gregorian/gregorian.hpp>

namespace date = boost::gregorian;
namespace date_time = boost::date_time;

double bondPrice(
    date::date maturityDate,
    double faceValue,
    double yield,
    double intrestRate,
    date::date priceDay
)
{
    using std::pow;

    int days = ( maturityDate - priceDay ).days();

    if( days < 0 ){
        return 0;
    }

    double result = 0;

    {
        /*
         * Face value
         */

        result = faceValue / pow( ( 1.0 + intrestRate ), 1.0 * days / 365 );

        std::cout << "  PV( F ): " << result << std::endl;
    }

    while( days > 0 )
    {
        /*
         * Coupons
         */

        double coupon =
            ( faceValue * yield ) / pow( ( 1.0 + intrestRate ), 1.0 * days / 365 );

        std::cout << "  PV( Coupon ): " << coupon << std::endl;

        result += coupon;

        maturityDate = maturityDate - date::years( 1 );

        days = ( maturityDate - priceDay ).days();
    }

    std::cout << "P: " << result << std::endl;

    return result;
}

int main( int argc, char* argv[] )
{
    if( argc != 6 )
    {
        std::cerr << "Usage: " << argv[0] << " maturityDate faceValue yield intrestRate priceDate" << std::endl;
        std::cerr << "       " << argv[0] << " 2015/12/31 100 0.08 0.08 2014/12/31" << std::endl;

        return 1;
    }

    bondPrice(
        date::from_string( argv[1] ),
        std::stoi( argv[2] ),
        std::stof( argv[3] ),
        std::stof( argv[4] ),
        date::from_string( argv[5] )
    );

    return 0;
}


#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>
#include "coordinate_transform.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <functional>

//----------------compVectorBegin----------------
//! Evaluate the load vector on the triangle spanned by
//! the points (a, b, c).
//!
//! Here, the load vector is a vector $(v_i)$ of
//! three components, where
//!
//! $$v_i = \int_{K} \lambda_i^K(x, y) f(x, y) \; dV$$
//!
//! where $K$ is the triangle spanned by (a, b, c).
//!
//! @param[out] loadVector should be a vector of length 3.
//!                        At the end, will contain the integrals above.
//!
//! @param[in] a the first corner of the triangle
//! @param[in] b the second corner of the triangle
//! @param[in] c the third corner of the triangle
//! @param[in] f the function f (LHS).
template<class Vector, class Point>
void computeLoadVector(Vector& loadVector,
                    const Point& a, const Point& b, const Point& c,
                    const std::function<double(double, double)>& f)
{
    Eigen::Matrix2d coordinateTransform = makeCoordinateTransform(b - a, c - a);
    double volumeFactor = std::abs(coordinateTransform.determinant());
// (write your solution here)

}
//----------------compVectorEnd----------------


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

#ifndef CBR_MATH__MATH_HPP_
#define CBR_MATH__MATH_HPP_

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <algorithm>
#include <array>
#include <cmath>
#include <exception>
#include <limits>
#include <type_traits>

namespace cbr
{

/***************************************************************************
 * \brief Converts degrees to radians
 ***************************************************************************/
template<typename T>
constexpr T deg2rad(const T deg) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return deg * M_PI / 180.;
}

/***************************************************************************
 * \brief Converts radians to degrees
 ***************************************************************************/
template<typename T>
constexpr T rad2deg(const T rad) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return rad * 180. / M_PI;
}

/***************************************************************************
 * \brief Computes b^e
 ***************************************************************************/
template<typename TBase, typename TExp>
constexpr TBase powFast(const TBase b, const TExp e) noexcept
{
  static_assert(std::is_unsigned_v<TExp>, "Exponent type must be unsigned.");

  if (e == 0) {
    return TBase(1.);
  }

  if (e == 1) {
    return b;
  }

  TBase out = b;
  for (TExp i = 2; i <= e; i++) {
    out *= b;
  }

  return out;
}

namespace detail
{

template<typename T, std::size_t ... Is>
constexpr T powFastImpl([[maybe_unused]] T val, std::index_sequence<Is...>)
{
  #pragma GCC diagnostic push
  #pragma GCC diagnostic ignored "-Wunused-value"
  #pragma GCC diagnostic ignored "-Wconversion"
  // *INDENT-OFF*
  return ((Is, val) * ... * T{1});
  // *INDENT-ON*
  #pragma GCC diagnostic pop
}

}  // namespace detail

// /***************************************************************************
//  * \brief Computes b^e via expansion to e-length product  b * ... * b
//  ***************************************************************************/
template<std::size_t e = 2, typename TBase>
constexpr TBase powFast(const TBase b) noexcept
{
  return detail::powFastImpl(b, std::make_index_sequence<e>{});
}

/***************************************************************************
* \brief Wraps angle in radians between 0 and 2*pi
***************************************************************************/
template<typename T>
T wrap2Pi(const T in) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return in - std::floor(in / T(2. * M_PI)) * T(2. * M_PI);
}

/***************************************************************************
* \brief Wraps angle in radians between -pi and pi
***************************************************************************/
template<typename T>
T wrapPi(const T in) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return wrap2Pi(in + M_PI) - M_PI;
}

/***************************************************************************
 * \brief Converts yaw in radians to heading in degrees
 ***************************************************************************/
template<typename T>
T yaw2heading(const T rad) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return rad2deg(wrap2Pi(-rad));
}

/***************************************************************************
 * \brief Computes truncated factorial : product from x0(default: 1) to x.
 ***************************************************************************/
template<typename T>
constexpr T factorial(const T x, const T x0 = T(1)) noexcept
{
  static_assert(std::is_unsigned_v<T>, "Type must be unsigned.");

  if (x == 0 || x0 > x) {
    return T(1);
  }

  if (x0 >= x) {
    return x;
  }

  T out = x0;
  for (T i = x0 + T(1); i <= x; i++) {
    out *= i;
  }
  return out;
}

/***************************************************************************
 * \brief Converts probability to log odds
 ***************************************************************************/
template<typename T>
constexpr T prob_to_log_odds(const T p)
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return log(p / (1. - p));
}

/***************************************************************************
 * \brief Converts to log odds to probability
 ***************************************************************************/
template<typename T>
constexpr T log_odds_to_prob(const T l)
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return 1. - (1. / (1. + exp(l)));
}

/***************************************************************************
 * \brief Converts a quaternion to euler angles using the ZYX decomposition
 * The returned vector contains the angles in [roll, pitch, yaw] order
 ***************************************************************************/
template<typename derived>
Eigen::Matrix<typename derived::Scalar, 3, 1>
quat2eulZYX(const Eigen::QuaternionBase<derived> & q) noexcept
{
  const auto qn = q.normalized();
  using T = typename derived::Scalar;
  Eigen::Matrix<T, 3, 1> eul;

  // roll
  eul[0] = atan2(
    T(2.) * (qn.w() * qn.x() + qn.y() * qn.z()),
    T(1.) - T(2.) * (powFast<2>(qn.x()) + powFast<2>(qn.y())));

  // pitch
  const T sinp = T(2.) * (qn.w() * qn.y() - qn.z() * qn.x());
  // Autodiff friendly manner of handling numerical errors
  if (abs(sinp) >= T(1.)) {
    if (sinp > T(0.)) {
      eul[1] = T(M_PI_2);
    } else {
      eul[1] = -T(M_PI_2);
    }
  } else {
    eul[1] = asin(sinp);
  }

  // yaw
  eul[2] = atan2(
    T(2.) * (qn.w() * qn.z() + qn.x() * qn.y()),
    T(1.) - T(2.) * (powFast<2>(qn.y()) + powFast<2>(qn.z())));

  return eul;
}

/***************************************************************************
 * \brief Converts a quaternion to euler angles using the ZYX decomposition
 * The returned vector contains the angles in [roll, pitch, yaw] order
 ***************************************************************************/
template<typename derived>
void quat2eulZYX(
  const Eigen::QuaternionBase<derived> & q,
  Eigen::Matrix<typename derived::Scalar, 3, 1> & eul) noexcept
{
  eul = quat2eulZYX<derived>(q);
}

/***************************************************************************
 * \brief Converts euler angles to a quaternion using the ZYX decomposition
 * The input vector must contain the angles in [roll, pitch, yaw] order
 ***************************************************************************/
template<typename T>
Eigen::Quaternion<T> eul2quatZYX(const Eigen::Matrix<T, 3, 1> & eul) noexcept
{
  Eigen::Quaternion<T> q;

  T cy = cos(eul[2] * .5);
  T sy = sin(eul[2] * .5);
  T cp = cos(eul[1] * .5);
  T sp = sin(eul[1] * .5);
  T cr = cos(eul[0] * .5);
  T sr = sin(eul[0] * .5);

  q.w() = cy * cp * cr + sy * sp * sr;
  q.x() = cy * cp * sr - sy * sp * cr;
  q.y() = sy * cp * sr + cy * sp * cr;
  q.z() = sy * cp * cr - cy * sp * sr;

  return q;
}

template<typename T, typename derived>
void eul2quatZYX(
  const Eigen::Matrix<T, 3, 1> & eul,
  Eigen::QuaternionBase<derived> & q) noexcept
{
  q = eul2quatZYX<T>(eul);
}

/***************************************************************************
 * \brief Converts a pure yaw to a quaternion
 ***************************************************************************/
template<typename T>
Eigen::Quaternion<T> yaw2quat(const T & yaw) noexcept
{
  return Eigen::Quaternion<T>(cos(yaw * T(.5)), T(0.0), T(0.0), sin(yaw * T(.5)));
}

/***************************************************************************
 * \brief Extract the yaw component of a quaternion
 ***************************************************************************/
template<typename derived>
typename derived::Scalar quat2yaw(const Eigen::QuaternionBase<derived> & q) noexcept
{
  using T = typename derived::Scalar;

  const auto qn = q.normalized();
  return atan2(
    T(2.) * (qn.w() * qn.z() + qn.x() * qn.y()),
    T(1.) - T(2.) * (powFast<2>(qn.y()) + powFast<2>(qn.z())));
}

/***************************************************************************
 * \brief Convert subscripts to linear indices
 ***************************************************************************/
template<typename T, std::size_t N>
constexpr T sub2ind(const std::array<T, N> & sz, const std::array<T, N> & idx)
{
  static_assert(std::is_arithmetic_v<T>, "Type must be arithmetic.");
  static_assert(N > 1, "Size of inputs must be > 1.");

  T ind = idx[0] + idx[1] * sz[0];

  if constexpr (N > 2) {
    T stride = sz[0];
    for (std::size_t i = 2; i < N; i++) {
      stride *= sz[i - 1];
      ind += idx[i] * stride;
    }
  }

  return ind;
}

/***************************************************************************
 * \brief Convert linear indices to subscripts
 ***************************************************************************/
template<typename T, std::size_t N>
constexpr std::array<T, N> ind2sub(const std::array<T, N> & sz, T idx)
{
  static_assert(std::is_arithmetic_v<T>, "Type must be arithmetic.");
  static_assert(N > 1, "Size of first input must be > 1.");

  std::array<T, N> sub{};

  if constexpr (N > 2) {
    std::array<T, N - 2> strides{};
    strides[0] = sz[0] * sz[1];
    for (std::size_t i = 1; i < N - 2; i++) {
      strides[i] = strides[i - 1] * sz[i + 1];
    }
    for (std::size_t i = N - 1; i > 1; i--) {
      sub[i] = idx / strides[i - 2];
      idx = idx % strides[i - 2];
    }
  }

  sub[1] = idx / sz[0];
  sub[0] = idx % sz[0];

  return sub;
}

/***************************************************************************
 * \brief Antisymmetric power function
 ***************************************************************************/
template<typename T1, typename T2>
auto powAntisym(T1 base, T2 exp)
{
  static_assert(std::is_arithmetic_v<T1>, "Base type must be arithmetic.");
  static_assert(std::is_arithmetic_v<T2>, "Exponent type must be arithmetic.");

  if (base >= T1(0)) {
    return std::pow(base, exp);
  } else {
    return -std::pow(-base, exp);
  }
}

/***************************************************************************
 * \brief Over-approximate a body bounding box with a world bounding box
 ***************************************************************************/
template<typename S, typename T, int I>
Eigen::AlignedBox<T, I> overapp_bbox(S && pose, Eigen::AlignedBox<T, I> bbox_B)
{
  Eigen::Array<T, I, 1> bbmin = std::numeric_limits<T>::max() * Eigen::Array<T, I, 1>::Ones();
  Eigen::Array<T, I, 1> bbmax = std::numeric_limits<T>::min() * Eigen::Array<T, I, 1>::Ones();
  for (int n = 0; n != pow(2, I); ++n) {  // find min/max coordinates of bounding box in world frame
    auto p = pose * bbox_B.corner(static_cast<typename Eigen::AlignedBox<T, I>::CornerType>(n));
    bbmin = bbmin.min(p.array());
    bbmax = bbmax.max(p.array());
  }
  return Eigen::AlignedBox<T, I>{bbmin, bbmax};
}

/***************************************************************************
 * \brief Smooth saturation function
 ***************************************************************************/
template<typename T1, typename T2>
constexpr void smoothSatInPlace(
  T1 & x,
  const T2 & mi,
  const T2 & ma,
  const T2 & satSmoothCoeff = T2(0.1))
{
  static_assert(std::is_floating_point_v<T2>, "Input type must be a floating point.");

  if (satSmoothCoeff < T2(0.) || T2(1.) < satSmoothCoeff) {
    throw std::invalid_argument("satSmoothCoeff must be between 0 and 1.");
  }
  if (ma < mi) {
    throw std::invalid_argument("max must be greated or equal to min.");
  }

  constexpr T2 alpha = M_PI / 8.;
  constexpr T2 beta = M_PI / 4.;
  const T2 r = satSmoothCoeff * M_SQRT2 / tan(alpha);
  const T2 bevelL = r * tan(alpha);
  const T2 bevelStart = 1. - cos(beta) * bevelL;
  const T2 bevelStop = 1. + bevelL;
  const T2 bevelXc = bevelStop;
  const T2 bevelYc = 1. - r;

  const T2 range = ma - mi;
  const T2 middle = (ma + mi) / 2.;
  const T1 uc = 2. * (x - middle) / range;

  if (uc >= bevelStop) {
    x = ma;
  } else if (uc <= -bevelStop) {
    x = mi;
  } else if (uc > bevelStart) {
    x = 0.5 * (sqrt(r * r - (uc - bevelXc) * (uc - bevelXc)) + bevelYc) * range + middle;
  } else if (uc < -bevelStart) {
    x = 0.5 * (-sqrt(r * r - (uc + bevelXc) * (uc + bevelXc)) - bevelYc) * range + middle;
  } else {
    return;
  }
}

template<typename T1, typename T2>
constexpr T1 smoothSat(
  const T1 & x,
  const T2 & mi,
  const T2 & ma,
  const T2 & satSmoothCoeff = T2{0.1})
{
  T1 out{x};
  smoothSatInPlace<T1, T2>(out, mi, ma, satSmoothCoeff);
  return out;
}

/***************************************************************************
 * \brief Check if point is inside polygon
 ***************************************************************************/
template<typename ForwardIterator, typename Point>
bool point_in_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const Point & point)
{
  ForwardIterator current = first;
  if (current == last) {return false;}  // empty list of vertices

  ForwardIterator next = current; ++next;
  if (next == last) {return false;}  // only 1 vertex

  ForwardIterator next_plus_1 = next; ++next_plus_1;
  if (next_plus_1 == last) {return false;}  // only 2 vertex

  auto which_side_in_slab =
    [](
    const auto & pt,
    const auto & low,
    const auto & high) -> int
    {
      const double cross_product =
        (high[0] - low[0]) * (pt[1] - low[1]) - (high[1] - low[1]) * (pt[0] - low[0]);

      if (cross_product > 0.) {
        return 1;
      }

      if (cross_product < 0.) {
        return -1;
      }

      return 0;
    };

  auto compare_x_2 = [](const auto & p1, const auto & p2) -> int {
      if (p1[0] < p2[0]) {
        return -1;
      } else if (p1[0] > p2[0]) {
        return 1;
      }
      return 0;
    };

  auto compare_y_2 = [](const auto & p1, const auto & p2) -> int {
      if (p1[1] < p2[1]) {
        return -1;
      } else if (p1[1] > p2[1]) {
        return 1;
      }
      return 0;
    };

  bool IsInside = false;
  int cur_y_comp_res = compare_y_2(*current, point);

  do {
    int next_y_comp_res = compare_y_2(*next, point);

    switch (cur_y_comp_res) {
      case -1:
        switch (next_y_comp_res) {
          case -1:
            break;
          case 0:
            switch (compare_x_2(point, *next)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
              case 1:
                break;
            }
            break;
          case 1:
            switch (which_side_in_slab(point, *current, *next)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
            }
            break;
        }
        break;
      case 0:
        switch (next_y_comp_res) {
          case -1:
            switch (compare_x_2(point, *current)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
              case 1:
                break;
            }
            break;
          case 0:
            switch (compare_x_2(point, *current)) {
              case -1:
                if (compare_x_2(point, *next) != -1) {
                  return true;
                }
                break;
              case 0:
                return true;
              case 1:
                if (compare_x_2(point, *next) != 1) {
                  return true;
                }
                break;
            }
            break;
          case 1:
            if (compare_x_2(point, *current) == 0) {
              return true;
            }
            break;
        }
        break;
      case 1:
        switch (next_y_comp_res) {
          case -1:
            switch (which_side_in_slab(point, *next, *current)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
            }
            break;
          case 0:
            if (compare_x_2(point, *next) == 0) {
              return true;
            }
            break;
          case 1:
            break;
        }
        break;
    }
    current = next;
    cur_y_comp_res = next_y_comp_res;
    ++next;
    if (next == last) {next = first;}
  } while (current != first);

  return IsInside;
}

/***************************************************************************
 * \brief Compute distance from point to segment
 ***************************************************************************/
template<typename Point_t>
constexpr auto point_to_segment_dist_squared(
  const Point_t & pt,
  const Point_t & seg_pt1,
  const Point_t & seg_pt2)
{
  using scalar_t = std::decay_t<decltype(pt[0])>;

  const scalar_t pt_x = pt[0] - seg_pt1[0];
  const scalar_t pt_y = pt[1] - seg_pt1[1];

  if (seg_pt1[0] == seg_pt2[0] && seg_pt1[1] == seg_pt2[1]) {
    return pt_x * pt_x + pt_y * pt_y;
  }

  const scalar_t seg_x = seg_pt2[0] - seg_pt1[0];
  const scalar_t seg_y = seg_pt2[1] - seg_pt1[1];

  const scalar_t l2 = seg_x * seg_x + seg_y * seg_y;
  const scalar_t proj = (pt_x * seg_x + pt_y * seg_y) / l2;
  const scalar_t t = std::min(std::max(proj, scalar_t(0.)), scalar_t(1.));

  const scalar_t dist_x = t * seg_x - pt_x;
  const scalar_t dist_y = t * seg_y - pt_y;

  return dist_x * dist_x + dist_y * dist_y;
}

/***************************************************************************
 * \brief Checks if 2 segments intersect
 ***************************************************************************/
template<typename Point_t>
constexpr bool segments_intersect(
  const Point_t & seg1_pt1,
  const Point_t & seg1_pt2,
  const Point_t & seg2_pt1,
  const Point_t & seg2_pt2)
{
  if (seg1_pt1[0] == seg1_pt2[0] && seg1_pt1[1] == seg1_pt2[1]) {
    throw std::invalid_argument("First segment has no length");
  }

  if (seg2_pt1[0] == seg2_pt2[0] && seg2_pt1[1] == seg2_pt2[1]) {
    throw std::invalid_argument("Second segment has no length");
  }
  using scalar_t = std::decay_t<decltype(seg1_pt1[0])>;

  const scalar_t seg1_x = seg1_pt2[0] - seg1_pt1[0];
  const scalar_t seg1_y = seg1_pt2[1] - seg1_pt1[1];

  const scalar_t seg2_x = seg2_pt2[0] - seg2_pt1[0];
  const scalar_t seg2_y = seg2_pt2[1] - seg2_pt1[1];

  const scalar_t det = seg1_y * seg2_x - seg1_x * seg2_y;
  if (det == scalar_t(0.)) {
    return false;
  }

  const scalar_t pt1_dx = seg2_pt1[0] - seg1_pt1[0];
  const scalar_t pt1_dy = seg2_pt1[1] - seg1_pt1[1];

  const scalar_t alpha = (pt1_dy * seg2_x - pt1_dx * seg2_y) / det;
  const scalar_t beta = (pt1_dy * seg1_x - pt1_dx * seg1_y) / det;

  if (
    alpha < scalar_t(0.) ||
    alpha > scalar_t(1.) ||
    beta < scalar_t(0.) ||
    beta > scalar_t(1.))
  {
    return false;
  }

  return true;
}

template<typename PointPair>
bool segments_intersect(
  const PointPair & seg1,
  const PointPair & seg2)
{
  return segments_intersect(seg1.first, seg1.second, seg2.first, seg2.second);
}

/***************************************************************************
 * \brief Checks if segment intersects a polygon
 ***************************************************************************/
template<typename ForwardIterator, typename Point>
bool segment_inter_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const Point & point1,
  const Point & point2)
{
  ForwardIterator current = first;
  ForwardIterator next = current; ++next;

  do {
    if (segments_intersect(point1, point2, *current, *next)) {
      return true;
    }

    current = next;
    ++next;
    if (next == last) {next = first;}
  } while (current != first);

  return false;
}

template<typename ForwardIterator, typename PointPair>
bool segment_inter_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const PointPair & seg)
{
  return segment_inter_polygon(first, last, seg.first, seg.second);
}

/***************************************************************************
 * \brief Checks if segment is in the interior of a polygon
 ***************************************************************************/
template<typename ForwardIterator, typename Point>
bool segment_in_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const Point & point1,
  const Point & point2)
{
  if (!point_in_polygon(first, last, point1)) {
    return false;
  }

  return !segment_inter_polygon(first, last, point1, point2);
}

template<typename ForwardIterator, typename PointPair>
bool segment_in_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const PointPair & seg)
{
  return segment_in_polygon(first, last, seg.first, seg.second);
}

/***************************************************************************
 * \brief Array of N evenly spaced numbers
 ***************************************************************************/
template<std::size_t N, typename T>
constexpr std::array<T, N> linspace(T x0, T xT)
{
  static_assert(std::is_floating_point_v<T>, "T must be floating-point number");
  static_assert(N >= 2, "N must be greater or equal to 2");

  T dx = (xT - x0) / static_cast<T>(N - 1);
  std::array<T, N> ret{x0};
  for (std::size_t i = 1; i != N; ++i) {
    ret[i] = x0 += dx;
  }
  return ret;
}

/***************************************************************************
 * \brief Sigmoid with center and steepness
 ***************************************************************************/
template<typename T>
T sigmoid(T x, T center, T k)
{
  static_assert(std::is_floating_point_v<T>, "T must be an arithmetic type");
  using std::exp;

  return T(1) / (T(1) + exp(-k * (x - center)));
}

namespace detail
{
constexpr Eigen::StorageOptions layout(size_t N)
{
  if (N > 1) {
    return Eigen::RowMajor;
  }
  return Eigen::ColMajor;
}
}   // namespace detail

/**
 * Calculate sample error covariance matrix \Sigma for a dataset and an error functions
 * @param data container with samples of type X, e.g. std::vector<X>
 * @param fcn mapping X -> ErrT where ErrT is an Eigen column vector/array of size D
 * @return covariance matrix of size D x D
 *
 *  \Sigma = (1/N-1) * \sum_i (fcn(x_i) - \bar x) * (fcn(x_i) - \bar x).transpose()
 *
 *   where  \bar x = (1/N) * \sum_i fcn(x_i)
 *
 */
template<typename DataContainerT, typename ErrorFcnT>
auto sample_covariance(
  const DataContainerT & data,
  ErrorFcnT && fcn = [](const auto & x) {return x;})
{
  using ResT = typename std::result_of_t<
    ErrorFcnT(typename DataContainerT::value_type)
    >::PlainMatrix;
  static_assert(ResT::ColsAtCompileTime == 1, "fcn must map to column vector");
  static_assert(std::is_base_of_v<Eigen::MatrixBase<ResT>, ResT>, "fcn must map to eigen type");

  static constexpr int N = ResT::RowsAtCompileTime;
  using InfT = Eigen::Matrix<typename ResT::Scalar, N, N>;

  InfT cov = InfT::Zero();

  if (data.size() > 1) {
    using CompT = Eigen::Matrix<
      typename ResT::Scalar, Eigen::Dynamic, N, detail::layout(N)
    >;
    CompT errs(data.size(), N);
    for (decltype(data.size()) i = 0; i != data.size(); ++i) {
      errs.row(static_cast<Eigen::Index>(i)) = fcn(data[i]);
    }

    CompT centered = errs.rowwise() - errs.colwise().mean();
    cov = (centered.adjoint() * centered) / (errs.rows() - 1);
  }

  return cov;
}

/**
 * Calculate sample square root information matrix I = \Sigma^{-1/2}
 * @param cov covariance
 * @param min_eig lower bound for eigenvalues of \Sigma (smaller eigenvalues are set to this value)
 * @return square root information matrix of size D x D
 */
template<typename Derived>
auto sqrt_information(const Eigen::MatrixBase<Derived> & cov, typename Derived::Scalar min_eig)
{
  Eigen::SelfAdjointEigenSolver<typename Derived::PlainMatrix> es(cov);
  return (es.eigenvectors() *
         es.eigenvalues().cwiseMax(min_eig).cwiseSqrt().cwiseInverse().asDiagonal() *
         es.eigenvectors().transpose()).eval();
}

}  // namespace cbr

#endif  // CBR_MATH__MATH_HPP_


/*
######################################################
Introduction to Criptography - 2021 - Homework2
Last modification (18-Mar-2021)

Author: Gaina Robert-Adrian
Contact: gainarobertadrian@gmail.com

Pseudo Random Number Generators:
1. Blum-Blum-Shub
2. Jacobi generator (a) with p&q b)with generalized components p1,p2,...pi)
######################################################

Resources:
1. https://pdfs.semanticscholar.org/c19b/91cdc1da67c52e606cd4752472ce0db83131.pdf
2. https://link.springer.com/content/pdf/10.1007/0-387-34799-2_13.pdf
3. https://libntl.org/ (Big number library)
4. https://en.wikipedia.org/wiki/Blum_Blum_Shub
5. https://asecuritysite.com/encryption/blum

*/
#include <NTL/ZZ.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <utility>

using namespace std;
using namespace NTL;

const int RUNS = 5;

class blum_blum_shub {
private:
	 ZZ p;
	 ZZ q;
	 ZZ m;
	 ZZ seed;
	 ZZ random;
	 int length;

	 ZZ primeCongruentTo3Mod4(int length) {
		 ZZ aux;
		 GenPrime(aux, length);
		 while (aux % 4 != 3) {
			 GenPrime(aux, length);
		 }
		 return aux;
	 }

	 ZZ seedGenerator(ZZ& p, ZZ& q) {
		 ZZ aux;
		 ZZ m = p * q;
		 aux = RandomBnd(m);
		 while (aux % p == 0 || aux % q == 0) {
			 aux = RandomBnd(m);
		 }
		 return aux;
	 }

public:
	blum_blum_shub(int& length) {
		this->length = length;
		p = primeCongruentTo3Mod4(length);
		q = primeCongruentTo3Mod4(length);
		m = p * q;
		seed = seedGenerator(p, q);
		random = seed;
	}

	ZZ random_bbs() {
		random = PowerMod(random, 2, m);
		return random;
	}

	void printAll() {
		cout << "------------------------------------------------------------------------------\n";
		cout << "P:" << p << '\n';
		cout << "Q:" << q << '\n';
		cout << "M:" << m << '\n';
		cout << "Seed:" << seed << '\n';
		cout << "Random:" << random << '\n';
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}

};

/*
1.Modulus division by a power-of-2-number can be optimized.
		 a % (power_of_2) == a & (power_of_2-1)
 2. Division and multiplication by powers of two can also be optimized by shifting bits.
		a / (power_of_2) == a >>= n where 2^n= power_of_2
 */
ZZ jacobi_calculator(ZZ a, ZZ n) {
	if (n % 2 == 0) {
		cout << "Second parameter must be odd";
		return (ZZ)0;
	}
	ZZ b = a % n;
	ZZ c = n;
	ZZ s = (ZZ)1;
	while (b >= 2) {

		while ((b & 3) == 0) {						// b % 4
			b >>= 2;								// b /= 4
		}
		if ((b & 1) == 0) {							// b % 2
			if ((c & 7) == 3 || (c & 7) == 5) {		// c % 8
				s = -s;
			}
			b >>= 1;								// b /= 2
		}
		if (b == 1) break;
		if ((b & 3) == 3 && (c & 3) == 3) {			// b % 4
			s = -s;
		}
		if (c % b == 0) {
			b = 0;
			break;
		}
		ZZ aux = b;
		b = c % b;
		c = aux;
	}
	return s * b;
}

class jacobi {
private:
	ZZ p;
	ZZ q;
	ZZ m;
	ZZ seed;
	ZZ random;
	vector<ZZ> m_components;
	int length;
	int l;
	vector<bool> generated_binary;
public:
	jacobi(int length, int l) {
		this->length = length;
		this->l = l;
		GenPrime(p, length);
		GenPrime(q, length);
		while (q % 2!=1) {
			GenPrime(q, length);
		}
		m = p * q;
	}

	jacobi(int NR_OF_PRIMES, int PRIME_LENGTH,bool second) {
		m = 1;
		for (int i = 0; i < NR_OF_PRIMES; ++i) {
			ZZ aux;
			GenPrime(aux, PRIME_LENGTH);
			m_components.push_back(aux);
			m *= aux;
		}
		l = PRIME_LENGTH * NR_OF_PRIMES;
	}

	vector<bool> generate_random(int LENGTH) {
		vector<ZZ> a_components;
		for (int i = 0; i < m_components.size(); ++i) {
			ZZ aux;
			aux = RandomBnd(m);
			a_components.push_back(aux);
		}
		ZZ seed = a_components[0];
		ZZ p = m_components[0];
		for (int i = 1; i < m_components.size(); ++i) {
			CRT(seed, p, a_components[i], m_components[i]);
		}
		seed = seed % m;
		if (seed < 0)
			seed += m;

		vector<bool> aux_vector;
		for (int i = 0; i < LENGTH; ++i) {
			ZZ aux = jacobi_calculator(seed + (ZZ)i, m);
			if (aux == -1)
				aux_vector.push_back(0);
			else if (aux == 1)
				aux_vector.push_back(1);
		}
		generated_binary = aux_vector;
		return generated_binary;
	}

	vector<bool> generate_random() {
		vector<bool> aux_vector;
		GenPrime(seed, RandomBnd(NumBits(m)));
		for (int i = 0; i < l; ++i) {
			ZZ aux = jacobi_calculator(seed + (ZZ)i, m);
			if (aux == -1)
				aux_vector.push_back(0);
			else if (aux == 1)
				aux_vector.push_back(1);
		}
		generated_binary = aux_vector;
		return generated_binary;
	}
};

vector<bool> convertToBinary(ZZ& number) {
	unsigned int number_size = NumBits(number);
	vector<bool> binary_representation(number_size);
	for (int i = 0; i < number_size; ++i) {
		binary_representation[number_size - (i + 1)] = bit(number, i);
	}
	return binary_representation;
}

void printBinary(const vector<bool>& v) {
	cout << "Binary format: ";
	for (int i = 0; i < v.size(); ++i)
		cout << v[i];
	cout << "\n\n";
}

void elementaryTest(const vector<bool>& v) {
	cout << "Elementary test:\n";
	int nr_of_0 = 0;
	for (int i = 0; i < v.size(); ++i) {
		if (v[i] == 0)
			++nr_of_0;
	}
	double percentage_of_0 = 100 * (double)nr_of_0 / v.size();
	cout << "Number of 0: " << nr_of_0<<'\n';
	cout << "Number of 1: " << v.size() - nr_of_0<<'\n';
	cout << "Percentages:\n0=" << percentage_of_0 << "%\n";
	cout << "1=" << 100 - percentage_of_0 << "%\n\n";
}

pair<string,string> printToFiles(const vector<bool>& v,const int& iteration,string generator) {
	string filename= generator + "_" + to_string(iteration)+".txt";
	string default_file = generator + "_Only1_" + to_string(iteration) + ".txt";
	ofstream fout(filename);
	ofstream f(default_file);
	for (int i = 0; i < v.size(); ++i)
	{
		fout << v[i];
		f << 1;
	}
	return make_pair(filename, default_file);
}

int getSize(ifstream& file) {
	file.seekg(0, ios::end);
	int size = file.tellg();
	file.seekg(0);
	return size;
}

void compressionTest(const vector<bool>& v,const int& i,string generator) {
	cout << "File compression test:\n";
	pair <string,string> files=printToFiles(v, i,generator);
	pair<string, string> zip_files = make_pair(files.first.substr(0, files.first.size() - 4)+".zip", files.second.substr(0, files.second.size() - 4)+".zip");
	system(("powershell Compress-Archive -Force " + files.first+ " " + zip_files.first).c_str());
	system(("powershell Compress-Archive -Force " + files.second + " " + zip_files.second).c_str());

	/*Default file size equals the generated one*/
	ifstream gin(files.first, ios::binary);
	ifstream zip_gin(zip_files.first, ios::binary);
	ifstream zip_din(zip_files.second, ios::binary);

	int g_size=getSize(gin);
	int zip_g_size=getSize(zip_gin);
	int zip_d_size=getSize(zip_din);

	double g_comp_ratio = ((double)zip_g_size / (double)g_size) * 100;
	double s_comp_ratio= ((double)zip_d_size / (double)g_size) * 100;

	printf("Initial files size: %d\nArchived file zie: %d\nDefault archived size: %d\n", g_size, zip_g_size, zip_d_size);
	printf("Generated file compression ratio: %.2f%%\n",g_comp_ratio);
	printf("Default file compression ratio: %.2f%%\n", s_comp_ratio);
}

void bbs_test(int bit_length) {
	blum_blum_shub generator(bit_length);
	ZZ a;
	generator.printAll();
	for (int i = 0; i < RUNS; ++i) {
		cout << "------------------------------------------------------------------------------\n";
		cout << "Iteration:" << i << "\t\t*bbs*\n";
		a = generator.random_bbs();
		cout << "Generated number=" << a << "\n\n";
		vector<bool> binary;
		binary = convertToBinary(a);
		printBinary(binary);
		elementaryTest(binary);
		compressionTest(binary,i,"bbs");
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}
}

void jacobi_test(int p_q_size,int nr_bit_length) {
	jacobi j(p_q_size, nr_bit_length);
	vector<bool> binary;
	for (int i = 0; i < RUNS; ++i) {
		cout << "------------------------------------------------------------------------------\n";
		cout << "Iteration:" << i << "\t*jacobi*\n";
		binary = j.generate_random();
		printBinary(binary);
		elementaryTest(binary);
		compressionTest(binary, i,"jcb");
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}
}

void jacobi_test_b(int NR_OF_PRIMES, int PRIME_LENGTH,int RANDOM_LENGTH) {
	jacobi j(NR_OF_PRIMES,PRIME_LENGTH,1);
	vector<bool> binary;
	for (int i = 0; i < RUNS; ++i) {
		cout << "------------------------------------------------------------------------------\n";
		cout << "Iteration:" << i << "\t*jacobi multiple components*\n";
		binary = j.generate_random(RANDOM_LENGTH);
		printBinary(binary);
		elementaryTest(binary);
		compressionTest(binary, i, "jcb_comp");
		cout << "------------------------------------------------------------------------------\n";
		cout << "\n\n\n";
	}
}

void jacobi_symbol_test(int bit_length,int runs) {
	blum_blum_shub generator(bit_length);
	ZZ a = generator.random_bbs();
	ZZ b = generator.random_bbs();
	while (b % 2 != 1) { b = generator.random_bbs(); }
	for (int i = 0; i < runs; ++i)
	{
		ZZ aux1, aux2;
		if ((aux1=jacobi_calculator(a, b)) != (aux2=Jacobi(a, b))) {
			cout << "Something is wrong";
			cout << aux1 << " " << aux2<<'\n';
			cout << "A:" << a << " B:" << b;
			return;
		}
		a = generator.random_bbs();
		b = generator.random_bbs();
		while (b % 2 != 1) { b = generator.random_bbs(); }
	}
	printf("%d symbols calculated correctly\n", runs);
}

int main()
{
	bbs_test(1024);
	jacobi_test(1024,2048);
	jacobi_test_b(5,512,2048);
}


#include <svgpp/svgpp.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION >= 106400
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace svgpp;
namespace ublas = boost::numeric::ublas;

typedef ublas::matrix<double> matrix_t;

struct TransformEventsPolicy
{
  typedef matrix_t context_type;

  static void transform_matrix(matrix_t & transform, const boost::array<double, 6> & matrix)
  {
    matrix_t m(3, 3);
    m <<=
      matrix[0], matrix[2], matrix[4],
      matrix[1], matrix[3], matrix[5],
      0, 0, 1;
    transform = ublas::prod(transform, m);
  }

  static void transform_translate(matrix_t & transform, double tx, double ty)
  {
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(0, 2) = tx; m(1, 2) = ty;
    transform = ublas::prod(transform, m);
  }

  static void transform_scale(matrix_t & transform, double sx, double sy)
  {
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(0, 0) = sx; m(1, 1) = sy;
    transform = ublas::prod(transform, m);
  }

  static void transform_rotate(matrix_t & transform, double angle)
  {
    angle *= boost::math::constants::degree<double>();
    matrix_t m(3, 3);
    m <<=
      std::cos(angle), -std::sin(angle), 0,
      std::sin(angle),  std::cos(angle), 0,
      0, 0, 1;
    transform = ublas::prod(transform, m);
  }

  static void transform_skew_x(matrix_t & transform, double angle)
  {
    angle *= boost::math::constants::degree<double>();
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(0, 1) = std::tan(angle);
    transform = ublas::prod(transform, m);
  }

  static void transform_skew_y(matrix_t & transform, double angle)
  {
    angle *= boost::math::constants::degree<double>();
    matrix_t m = ublas::identity_matrix<double>(3, 3);
    m(1, 0) = std::tan(angle);
    transform = ublas::prod(transform, m);
  }
};

int main()
{
  matrix_t transform(ublas::identity_matrix<double>(3, 3));
  value_parser<
    tag::type::transform_list,
    transform_policy<policy::transform::minimal>,
    transform_events_policy<TransformEventsPolicy>
  >::parse(tag::attribute::transform(), transform,
    std::string("translate(-10,-20) scale(2) rotate(45) translate(5,10)"), tag::source::attribute());
  std::cout << transform << "\n";
  return 0;
}


/*!
 * \file misc.cpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 21, 2019: created
 */

#pragma once

#include <type_traits>
#include <Eigen/Dense>

/*************************
 *** Utility functions ***
 *************************/

/*
double cross2D(Eigen::Ref<Eigen::Vector2d> const &x, Eigen::Ref<Eigen::Vector2d> const &y)
{
    return x(0)*y(1)-x(1)*y(0);
}
*/

inline double cross2D(Eigen::Vector2d &&x, Eigen::Vector2d &&y)
{
    return x(0)*y(1)-x(1)*y(0);
}

template<class T, int n, size_t... is>
Eigen::Matrix<T,n+1,1> affWrap_impl(Eigen::Matrix<T,n,1> const &mat, std::index_sequence<is...>)
{
    return Eigen::Matrix<T,n+1,1>{mat(is)...,1.0};
}

template<class T,int n>
Eigen::Matrix<T,n+1,1> affWrap(Eigen::Matrix<T,n,1> const &mat)
{
    return affWrap_impl(mat, std::make_index_sequence<n>());
}

//! Check if two triangles overwraps with each other.
//! Based on the idea suggested in https://stackoverflow.com/questions/2778240/detection-of-triangle-collision-in-2d-space.
inline bool trianglesIntersection2D(std::array<Eigen::Vector2d,3> const &t1_, std::array<Eigen::Vector2d,3> const &t2_)
{
    /*** Preparation ***/
    // Rotation matrix of M_PI/2 in radian.
    Eigen::Matrix<double,2,2> mat;
    mat << 0, 1, -1, 0;

    // We may assume the vertices are given counter-clockwisely around triangles.
    bool is_ccwise1 = static_cast<double>((t1_[1]-t1_[0]).adjoint()*mat*(t1_[2]-t1_[0])) > 0;
    bool is_ccwise2 = static_cast<double>((t2_[1]-t2_[0]).adjoint()*mat*(t2_[2]-t2_[0])) > 0;
    std::array<std::array<Eigen::Vector2d const*, 3>,2> t{
        &(t1_[0]),
        is_ccwise1 ? &(t1_[1]) : &(t1_[2]),
        is_ccwise1 ? &(t1_[2]) : &(t1_[1]),
        &(t2_[0]),
        is_ccwise2 ? &(t2_[1]) : &(t2_[2]),
        is_ccwise2 ? &(t2_[2]) : &(t2_[1])
    };

    /*** The algorithm begins here ***/

    // Find an edge in one triangle which separates the opposite vertex and the vertices of the other triangle.
    // If found, this means two triangles are disjoint.
    for(size_t i = 0; i < 3; ++i) {
        Eigen::RowVector2d rv = (*(t[0][(i+1)%3])-*(t[0][i])).adjoint()*mat;
        if (static_cast<double>(rv*(*(t[1][0])-*(t[0][i]))) < 0
            && static_cast<double>(rv*(*(t[1][1])-*(t[0][i]))) < 0
            && static_cast<double>(rv*(*(t[1][2])-*(t[0][i]))) < 0)
            return false;
    }

    for(size_t i = 0; i < 3; ++i) {
        Eigen::RowVector2d rv = (*(t[1][(i+1)%3])-*(t[1][i])).adjoint()*mat;
        if (static_cast<double>(rv*(*(t[0][0])-*(t[1][i]))) < 0
            && static_cast<double>(rv*(*(t[0][1])-*(t[1][i]))) < 0
            && static_cast<double>(rv*(*(t[0][2])-*(t[1][i]))) < 0)
            return false;
    }

    return true;
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_SIGNEDVOLUME_HPP
#define MCL_SIGNEDVOLUME_HPP 1

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

namespace mcl
{

static inline double signed_triangle_area(
	const Eigen::Vector2d &p1,
	const Eigen::Vector2d &p2,
	const Eigen::Vector2d &p3);

// Returns (unscaled) first derivative of signed triangle area
static inline std::vector<Eigen::Vector2d> signed_triangle_area_gradients(
	const Eigen::Vector2d &p1,
	const Eigen::Vector2d &p2,
	const Eigen::Vector2d &p3);

static inline double triangle_perimeter(
	const Eigen::Vector2d &p1,
	const Eigen::Vector2d &p2,
	const Eigen::Vector2d &p3);

static inline double triangle_area(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3);

static inline double signed_tet_volume(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3,
	const Eigen::Vector3d &p4);

// Returns (unscaled) first derivative of signed tet volume
static inline std::vector<Eigen::Vector3d> signed_tet_volume_gradients(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3,
	const Eigen::Vector3d &p4);

static inline double tet_surface_area(
	const Eigen::Vector3d &p1,
	const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3,
	const Eigen::Vector3d &p4);

// Probably not where this belongs but oh well.
// Returns the faces of a tet
static inline std::vector<Eigen::Vector3i>
	faces_from_tet(const Eigen::RowVector4i &t);

//
// Implementation
//

inline double signed_triangle_area(const Eigen::Vector2d &p1, const Eigen::Vector2d &p2, const Eigen::Vector2d &p3)
{
	return 0.5 * ( -p2[0]*p1[1] + p3[0]*p1[1] + p1[0]*p2[1] - p3[0]*p2[1] - p1[0]*p3[1] + p2[0]*p3[1] );
}

inline std::vector<Eigen::Vector2d> signed_triangle_area_gradients(
	const Eigen::Vector2d &a, const Eigen::Vector2d &b, const Eigen::Vector2d &c)
{
	std::vector<Eigen::Vector2d> g(3);
	g[0] = 0.5 * Eigen::Vector2d(b[1]-c[1], -b[0]+c[0]);
	g[1] = 0.5 * Eigen::Vector2d(-a[1]+c[1], a[0]-c[0]);
	g[2] = 0.5 * Eigen::Vector2d(a[1]-b[1], -a[0]+b[0]);
	return g;
}

inline double triangle_perimeter(const Eigen::Vector2d &p1, const Eigen::Vector2d &p2, const Eigen::Vector2d &p3)
{
	return (p1-p2).norm() + (p2-p3).norm() + (p3-p1).norm();
}

// https://en.wikipedia.org/wiki/Heron%27s_formula
inline double triangle_area(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, const Eigen::Vector3d &p3)
{
	double a = (p1-p2).norm();
	double b = (p2-p3).norm();
	double c = (p3-p1).norm();
	double s = (a+b+c) * 0.5;
	return std::sqrt(s*(s-a)*(s-b)*(s-c));
}

inline double signed_tet_volume(
	const Eigen::Vector3d &p1, const Eigen::Vector3d &p2,
	const Eigen::Vector3d &p3, const Eigen::Vector3d &p4)
{
	Eigen::Matrix3d edges;
	edges.col(0) = p2 - p1;
	edges.col(1) = p3 - p1;
	edges.col(2) = p4 - p1;
	return (1.0/6.0) * edges.determinant();
}

inline std::vector<Eigen::Vector3d> signed_tet_volume_gradients(
	const Eigen::Vector3d &a, const Eigen::Vector3d &b,
	const Eigen::Vector3d &c, const Eigen::Vector3d &d)
{
	std::vector<Eigen::Vector3d> grads(4);
	const Eigen::Vector3d &p0 = a;
	const Eigen::Vector3d &p1 = b;
	const Eigen::Vector3d &p2 = c;
	const Eigen::Vector3d &p3 = d;
	static const double sixth = (1.0/6.0);
	grads[0] = sixth * (p1 - p2).cross(p3 - p2);
	grads[1] = sixth * (p2 - p0).cross(p3 - p0);
	grads[2] = sixth * (p0 - p1).cross(p3 - p1);
	grads[3] = sixth * (p1 - p0).cross(p2 - p0);
	return grads;
}

inline double tet_surface_area(
		const Eigen::Vector3d &p1,
		const Eigen::Vector3d &p2,
		const Eigen::Vector3d &p3,
		const Eigen::Vector3d &p4)
{
	double a1 = triangle_area(p2,p3,p4);
	double a2 = triangle_area(p2,p3,p1);
	double a3 = triangle_area(p3,p4,p1);
	double a4 = triangle_area(p4,p2,p1);
	return (a1+a2+a3+a4);
}

inline std::vector<Eigen::Vector3i> faces_from_tet(const Eigen::RowVector4i &t)
{
	using namespace Eigen;
	std::vector<Vector3i> f = {
		Vector3i(t[0], t[1], t[3]),
		Vector3i(t[0], t[2], t[1]),
		Vector3i(t[0], t[3], t[2]),
		Vector3i(t[1], t[2], t[3]) };
	return f;
}

} // ns mcl

#endif


#include <tuple>
#include <complex>
# include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

tuple<MatrixXcd, MatrixXcd> qr_reduced(const MatrixXcd& A)
{
    // reduced QR decomposition using Modified Gram-Schmidt with Reortogonalization
    // reference: W. Ganter 1980, Algorithms for QR-Decomposition, research report no.80-02
    // A is  m x n  complex matrix
    int n= A.cols();
    MatrixXcd R= MatrixXcd::Zero(n,n);
    MatrixXcd Q= A;
    for(int k= 0; k < n; ++k){
        complex<double> tt {(0.0,0.0)};
        for(int j= 0; j < 2; ++j){
            for(int i= 0; i < k; ++i){
                complex<double> s= Q.col(i).adjoint()*Q.col(k);
                if(tt == (0.0,0.0)) R(i,k)= s;
                Q.col(k)= Q.col(k)-s*Q.col(i);
            }
            tt= Q.col(k).norm();
        }
        R(k,k)= tt;
        Q.col(k)= Q.col(k)/R(k,k);
    }
    return forward_as_tuple(Q,R);
}


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   PlaneFitter.hpp
 * Author: Jordan McManus
 */

#ifndef PLANEFITTER_HPP
#define PLANEFITTER_HPP

#include <Eigen/Dense>

class PlaneFitter {
public:

    static void convertPlaneZform2GeneralForm(Eigen::Vector3d & zForm, Eigen::Vector4d & generalForm) {
        /*
         * Z-form is z = Ax + By + C
         * General form is ax + by + cz + d = 0 with (a*a + b*b + c*c = 1 i.e. unit normal vector)
         */

        double c = 1.0 / zForm.norm();
        double a = -zForm(0) * c;
        double b = -zForm(1) * c;
        double d = -zForm(2) * c;
        double f = sqrt(a * a + b * b + c * c);
        a = a / f;
        b = b / f;
        c = c / f;
        d = d / f;

        generalForm << a, b, c, d;
    }

    static void convertPlaneGeneralForm2Zform(Eigen::Vector4d & generalForm, Eigen::Vector3d & zForm) {
        /*
         * Z-form is z = Ax + By + C
         * General form is ax + by + cz + d = 0 with (a*a + b*b + c*c = 1 i.e. unit normal vector)
         */
        double f = 1.0 / generalForm(2);
        double A = -generalForm(0) * f;
        double B = -generalForm(1) * f;
        double C = -generalForm(3) * f;

        zForm << A, B, C;
    }

    static void calculatePlaneResidualsFromMatrix(Eigen::VectorXd & residuals, Eigen::MatrixXd & cloud, Eigen::Vector4d & planarGeneralForm) {
        /*
         * General form is ax + by + cz + d = 0 with (a*a + b*b + c*c = 1 i.e. unit normal vector)
         */

        Eigen::MatrixXd augmentedCloud(cloud.rows(), 4);
        augmentedCloud.col(0) = cloud.col(0);
        augmentedCloud.col(1) = cloud.col(1);
        augmentedCloud.col(2) = cloud.col(2);
        augmentedCloud.col(3) = Eigen::VectorXd::Ones(cloud.rows());

        residuals = augmentedCloud*planarGeneralForm;
    }

    static void fitPlane(Eigen::MatrixXd & xyz, Eigen::Vector4d & planeGeneralFormParams) {

        Eigen::MatrixXd A(xyz.rows(), 3);
        A.col(0) = xyz.col(0);
        A.col(1) = xyz.col(1);
        A.col(2) = Eigen::VectorXd::Ones(xyz.rows());

        Eigen::VectorXd b = xyz.col(2);

        Eigen::Vector3d planeParameterEstimation = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

        convertPlaneZform2GeneralForm(planeParameterEstimation, planeGeneralFormParams);
    }

    static void fitPlane(Eigen::MatrixXd & A, Eigen::VectorXd & b, Eigen::Vector4d & planeGeneralFormParams) {
        Eigen::Vector3d planeParameterEstimation = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
        convertPlaneZform2GeneralForm(planeParameterEstimation, planeGeneralFormParams);
    }

};

#endif /* PLANEFITTER_HPP */


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
//#include <unsupported/Eigen/MatrixFunctions>

//Euler angles to DCM
//e = [phi, theta, psi]
//3-1-2 rotation
//ref: Todd Humphreys euler2dcm MATLAB function, "Aerial Robotics," 2019
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f euler2dcm(Vector3f e)
{
  std::cout << "Calculating trig values..." << std::endl;
  float cPhi = cos(e(0));
  float sPhi = sin(e(0));
  float cThe = cos(e(1));
  float sThe = sin(e(1));
  float cPsi = cos(e(2));
  float sPsi = sin(e(2));
  std::cout << "Building DCM..." << std::endl;
  Matrix3f DCM;
  DCM << (cPhi*cThe - sPhi*sPsi*sThe), (cThe*sPsi + cPsi * sPhi*sThe), (-cPhi*sThe),
         (-cPhi*sPsi),                                    (cPhi*cPsi),         sPhi,
         (cPsi*sThe + cThe*sPhi*sPsi), (sPsi*sThe - cPsi*cThe*sPhi),    (cPhi*cThe);
  return DCM;
}

int main()
{
  using Eigen::Vector3f;
  using Eigen::Matrix3f;
  using Eigen::Quaternionf;
  /*Matrix3f A;
  A << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  std::cout << (A*5) << std::endl;*/
  Vector3f v(1.5708, -1.5708, 0.7854);
  Matrix3f DCM = euler2dcm(v);
  std::cout << DCM << std::endl;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
//  Copyright Paul A. Bristow 2015 - 2016.
//  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)

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// For additional details, see C.M. Kormanyos,
// Real-Time C++: Efficient Object-Oriented and
// Template Microcontroller Programming (Springer, Heidelberg, 2013).
// in Section 12.7 and Chapter 13.
// See http://link.springer.com/chapter/10.1007/978-3-642-34688-0_13

#define USE_BARE_METAL

#include <cstdint>

#if defined(USE_BARE_METAL)
#else
  #include <iostream>
  #include <iomanip>
#endif

// Configure Boost.Fixed_point for a bare-metal system.

#if defined(USE_BARE_METAL)
  #define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION // Do not use Boost.Multiprecision.
  #define BOOST_FIXED_POINT_DISABLE_IOSTREAM       // Do not use I/O streaming.
#endif

#include <boost/fixed_point/fixed_point.hpp>

namespace local
{
  /*! Evaluate first derivative of real_function
      using a three-point central-difference rule of O(dx^6), for more details
     \see http://www.boost.org/doc/libs/release/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/fp_eg/nd.html.

    \tparam RealValueType Type of value, for example, a fixed_point_type.
    \tparam RealFunctionType Type of parameter @c real function.
    \param x Value of x.
    \param dx Step size.
    \param real_function Real Function.
    \return First derivative at x value.
  */

  //[fixed_point_derivative_function
  template<typename RealValueType,
           typename RealFunctionType>
  RealValueType first_derivative(const RealValueType& x,
                                 const RealValueType& dx,
                                 RealFunctionType real_function)
  {
    const RealValueType dx2(dx  + dx);
    const RealValueType dx3(dx2 + dx);

    const RealValueType m1((  real_function(x + dx)
                            - real_function(x - dx))  / 2U);
    const RealValueType m2((  real_function(x + dx2)
                            - real_function(x - dx2)) / 4U);
    const RealValueType m3((  real_function(x + dx3)
                            - real_function(x - dx3)) / 6U);

    const RealValueType fifteen_m1(m1 * 15U);
    const RealValueType six_m2    (m2 *  6U);
    const RealValueType ten_dx    (dx * 10U);

    return ((fifteen_m1 - six_m2) + m3) / ten_dx;
  }
//] [/fixed_point_derivative_function]
} // namespace local

// Implement a tiny simulated subset of the mcal (microcontroller abstraction layer).
namespace mcal
{
  namespace wdg
  {
    void trigger();
  }
} // namespace mcal::wdg

void mcal::wdg::trigger()
{
  // Simulate a fake watchdog trigger mechanism doing nothing here.
}

// Declare a global Boolean test variable.
bool global_result_is_ok;

extern "C" int main()
{
//[fixed_point_derivative_coeffic
  typedef boost::fixed_point::negatable<6, -9> fixed_point_type;

  const fixed_point_type a = fixed_point_type(12) / 10;
  const fixed_point_type b = fixed_point_type(34) / 10;
  const fixed_point_type c = fixed_point_type(56) / 10;

  // Compute the approximate derivative of (a * x^2) + (b * x) + c
  // evaluated at 1/2, where the approximate values of the coefficients
  // are: a = 1.2, b = 3.4, and c = 5.6. The numerical tolerance is set
  // to a value of approximately 1/4.
//] [/fixed_point_derivative_coeffic]
  // See http://link.springer.com/chapter/10.1007/978-3-642-34688-0_12 page 219-220.

//[fixed_point_derivative_evalution
  const fixed_point_type d =
    local::first_derivative(fixed_point_type(1) / 2, // x
                            fixed_point_type(1) / 4,  // Step size dx.
                            [&a, &b, &c](const fixed_point_type& x) -> fixed_point_type
                            {
                              return (((a * x) + b) * x) + c;
                            });
//] [/fixed_point_derivative_evalution]

  // The expected result is ((2 * a) + b) = (2.4 + 3.4) = 4.6 (exact).
  // We obtain a fixed-point result of approximately 4.5938.

  // Verify that the result lies within (4.5 < result < 4.7).
  // The expected result is 4.6, so this is a wide tolerance.

//[fixed_point_verify
  global_result_is_ok = ((d > (fixed_point_type(45) / 10)) && (d < (fixed_point_type(47) / 10)));
//] [/fixed_point_verify]

  #if defined(USE_BARE_METAL)
    // We can not print the fixed-point number to the output stream
    // because I/O-streaming is disabled for fixed-point in this
    // Boost configuration.
  #else
    // But if we could print to the output stream, it might look
    // similar to the lines below. When attempting to print to
    // the output stream, however, we would need to add <iostream>
    // and deactivate #define BOOST_FIXED_POINT_DISABLE_IOSTREAM.
    std::cout << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
              << std::fixed
              << d
              << std::endl;
  #endif

  // Is the result of taking the derivative of the quadratic function OK?
  if(global_result_is_ok)
  {
    // Here we could take some action in the microcontroller
    // such as toggle a digital output port to high, indicating
    // success of the test case.
  }

  #if defined(USE_BARE_METAL)
    // In this bare-metal OS-less system, do not return from main().
    for(;;)
    {
      mcal::wdg::trigger();
    }
  #endif
}


#include <iostream>
#include <eigen3/Eigen/Dense>
#include <vector>
#include <cmath>
#include <boost/numeric/odeint.hpp>
#include "gnuplot-iostream.h"
#include "system_function.h"
namespace odeint = boost::numeric::odeint;
int main()
{
	state_type x(2); // a vector with size 2.
	x[0] = 1.0;		 // start at x=1.0, p=0.0
	x[1] = 0.0;
	std::vector<state_type> x_vec;
	std::vector<double> times;

	size_t steps = odeint::integrate(harmonic_oscillator,
									 x, 0.0, 10.0, 0.1,
									 push_back_state_and_time(x_vec, times));

	std::vector<double> position;
	for (size_t i = 0; i <= steps; i++)
	{
		position.push_back(x_vec[i][0]);
	}
	/* output */
	Gnuplot gp;
	gp << "plot '-' using 1:2 with linespoint" << std::endl;
	gp.send1d(std::make_tuple(times, position));

	// for (size_t i = 0; i <= steps; i++)
	// {
	// 	std::cout << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
	// }

	return 0;
}


#include <limits>
#include <numeric>

#include <Eigen/Core>

#include "UtilMath.hpp"

namespace RGM
{

template<typename T>
T MathUtil_<T>::median(std::vector<T> & vData, int num/*=std::numeric_limits<int>::max()*/, bool sortData/*=false*/)
{

    if (vData.size()==0 || num==0) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    num = std::max<int>(std::min<int>(vData.size(), num), 1);

    if ( vData.size()==1 || num==1 ) {
        return vData[0];
    }

    std::vector<T> * p=0;

    if ( sortData ) {
        std::sort(vData.begin(), vData.begin()+num); // Ascending

        if ( num % 2 == 0) {
            int idx = num / 2;
            return (vData[idx-1] + vData[idx]) / (T)2;
        } else {
            int idx = (num-1) / 2;
            return vData[idx];
        }
    } else {
        std::vector<T> tmp(num);
        std::copy(vData.begin(), vData.begin()+num, tmp.begin());
        std::sort(tmp.begin(), tmp.end());

        if ( num % 2 == 0) {
            int idx = num / 2;
            return (tmp[idx-1] + tmp[idx]) / (T)2;
        } else {
            int idx = (num-1) / 2;
            return tmp[idx];
        }
    }
}

template<typename T>
std::vector<float> MathUtil_<T>::pdist(const std::vector<cv::Point_<T> > & pts, int num)
{

    std::vector<float> dist;

    num = std::min<int>(pts.size(), num);

    for ( int i=0; i<num-1; ++i ) {
        const cv::Point_<T> & pt1( pts[i] );
        for ( int j=i+1; j<num; ++j ) {
            const cv::Point_<T> & pt2( pts[j] );
            dist.push_back( sqrt((float)(pt1.x-pt2.x)*(pt1.x-pt2.x)+(pt1.y-pt2.y)*(pt1.y-pt2.y)) );
        }
    }

    return dist;
}

template<typename T>
std::vector<T> MathUtil_<T>::linspace(T s, T e, T interval)
{

    std::vector<T> x;

    for ( T i=s; i<=e; i+=interval ) {
        x.push_back(i);
    }

    return x;
}

template<typename T>
std::vector<T> MathUtil_<T>::hist(std::vector<T> & y, std::vector<T> & x)
{

    std::vector<T> n(x.size(), 0);

    std::vector<T> xx(x.size()+1);
    xx[0]           = -std::numeric_limits<T>::infinity();
    std::copy(x.begin(), x.end(), xx.begin()+1);

    for ( int i=0; i<y.size(); ++i ) {

        int j=0;
        for ( ; j<xx.size()-1; ++j ) {
            if (y[i]>xx[j] && y[i]<=xx[j+1]) {
                break;
            }
        }

        n[j]++;
    }

    return n;
}

template<typename T>
std::vector<T> MathUtil_<T>::convnSame(std::vector<T> & x, std::vector<T> & filter)
{

    std::vector<T> xx(x.size()+2*filter.size(), 0);
    std::copy(x.begin(), x.end(), xx.begin()+filter.size());

    std::reverse(filter.begin(), filter.end());

    std::vector<T> result(xx.size()-filter.size());
    for ( int i=0; i<result.size(); ++i) {
        result[i] = std::inner_product(filter.begin(), filter.end(), xx.begin()+i, 0.0F);
    }

    std::vector<T> r(x.size());

    int istart = floor(filter.size()/2.0F);
    std::copy(result.begin()+istart, result.begin()+istart+x.size(), r.begin());

    return r;
}

template<typename T>
T MathUtil_<T>::calcErr(std::vector<T> & pscores, std::vector<T> & nscores, T & thr)
{
    T minerr = 1.0;

    int numpos = pscores.size();
    int numneg = nscores.size();
    int num = numpos+numneg;

    std::vector<std::pair<T, int> > scores(num);

    for ( int i=0; i<numpos; ++i ) {
        scores[i] = std::pair<T, int>(-pscores[i], i);
    }
    for ( int i=numpos, j=0; j<numneg; ++i, ++j ) {
        scores[i] = std::pair<T, int>(-nscores[j], i);
    }

    std::sort(scores.begin(), scores.end());

    std::vector<int> tp(num, 0), fp(num, 0);

    for (int i=0; i<num; ++i ) {
        if ( scores[i].second < numpos ) {
            tp[i] = 1;
            fp[i] = 0;
        } else {
            fp[i] = 1;
            tp[i] = 0;
        }
    }

    Eigen::VectorXd tpr(num), fpr(num), err(num);

    tpr(0) = tp[0];
    fpr(0) = fp[0];
    for ( int i=1; i<num; ++i ) {
        tpr(i) = tpr(i-1) + tp[i];
        fpr(i) = fpr(i-1) + fp[i];
    }

    tpr /= numpos;
    // to fnr
    tpr.array() = 1 - tpr.array();
    fpr /= numneg;

    err = tpr+fpr;

    int r, c;
    minerr = err.minCoeff(&r, &c) / 2.0f;

    thr = std::max<T>(-scores[r].first, *std::min_element(pscores.begin(), pscores.end()));

    return minerr;
}

template<typename T>
T MathUtil_<T>::calcVar(std::vector<T> & pscores, T & thr)
{
    int num = pscores.size();

    //thr = std::numeric_limits<T>::max();

    T  m = 0;
    for ( int i=0; i<num; ++i) {
        m += pscores[i];
        //thr = std::min<T>(thr, pscores[i]);
    }

    m /= num;

    T v = 0;
    for ( int i=0; i<num; ++i) {
        v += std::pow(pscores[i]-m, 2.0F);
    }

    v /= (num-1);

    v = sqrt(v);

    thr = m - v;

    return v;
}

/// Specification
template class MathUtil_<int>;
template class MathUtil_<float>;
template class MathUtil_<double>;

} // namespace RGM


#pragma once

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

#include "topology.hpp"

using namespace Eigen;

/**
 * @brief Calcola la direzione normale di ogni corner 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 corner di ogni triangolo come una somma pesata delle normali
 * dei triangoli adiacenti nello stesso settore.
 * Ogni triangolo ha 3 corner.
 * Un settore e' costituito da un insieme di triangoli incidenti su uno stesso
 * vertice, uno adiacente al successivo, che approssimano una superficie smooth:
 * due triangoli adiacenti tra loro (incidono sullo stesso lato) fanno parte
 * dello stesso settore se l'angolo fra le loro rispettive direzioni normali e'
 * inferiore ad una data soglia (il coseno, o prodotto scalare, - e' maggiore
 * di una soglia).
 * Il peso è dato dall'area del triangolo moltiplicato l'angolo incidente sul
 * vertice.
 *
 * L'area di un triangolo viene calcolata come meta' della lunghezza (norma)
 * del prodotto vettoriale tra due lati.
 * L'angolo incidente su un vertice viene calcolato come l'arcocoseno del
 * prodotto scalare tra i due lati incidenti (intesi come vettori unitari
 * centrati su di esso), oppure come l'arcotangente del seno e coseno:
 * - il seno e' proporzionale alla normal del prodotto vettoriale
 * - il cose e' proporzionale al prodotto scalare.
 *
 * @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 tre righe per ogni triangolo
 *                  (N.rows() == F.rows() * 3) e 3 colonne. Ogni riga corrisponde
 *                  alla direzione normale di un corner di un triangolo, avente
 *                  le 3 coordinate x,y,z.
 */
MatrixXd perCornerNormals(MatrixXd const &V, MatrixXi const &F)
{
    MatrixXd N = MatrixXd::Zero(F.rows() * 3, 3);

    // coseno di 30 gradi:
    const double cos_thr = std::sqrt(3) / 2;

    // suggerimento: e' necessario trovare le facce adiacenti intorno ad un vertice
    // se non vuoi scrivere il codice che fa questo, puoi includere il file
    // #include "topology.hpp"
    // e chiamare le funzioni `vertex_face_adjacency()` e `face_face_adjacency()`

    std::vector<std::vector<int>> VF, VFi;
    MatrixXi FF, FFi;

    vertex_face_adjacency(V, F, VF, VFi);
    face_face_adjacency(V, F, VF, VFi, FF, FFi);

    MatrixXd FN(F.rows(), 3);
    VectorXd Fareas(F.rows());
    MatrixXd Fangles(F.rows(), 3);

    for (int f = 0; f < F.rows(); ++f)
    {
        int i = F(f, 0);
        int j = F(f, 1);
        int k = F(f, 2);

        Vector3d A = V.row(i);
        Vector3d B = V.row(j);
        Vector3d C = V.row(k);

        Vector3d e0 = B - A;
        Vector3d e1 = C - B;
        Vector3d e2 = A - C;

        Vector3d c0 = e0.cross(-e2);
        Vector3d c1 = e1.cross(-e0);
        Vector3d c2 = e2.cross(-e1);

        Fareas(f) = c0.norm() / 2.0;

        FN.row(f) = c0.normalized();

        Fangles(f, 0) = std::atan2(c0.norm(), e0.dot(-e2));
        Fangles(f, 1) = std::atan2(c1.norm(), e1.dot(-e0));
        Fangles(f, 2) = std::atan2(c2.norm(), e2.dot(-e1));
    }

    MatrixXd FF_cosines(F.rows(), 3);
    for (int f = 0; f < F.rows(); ++f) {
        auto const& fn = FN.row(f);
        for (int p = 0; p < 3; ++p) {
            auto const& ffn = FN.row(FF(f, p));
            FF_cosines(f, p) = fn.dot(ffn);
        }
    }

    for (int f = 0; f < F.rows(); ++f) {
        auto const& fn = FN.row(f);
        for (int p = 0; p < 3; ++p) {
            auto n = Fareas(f) * Fangles(f, p) * fn;

            N.row(3 * f + p) += n;

            // contribusci alle normali di tutte le facce adiacenti intorno
            // al corner p, il cui angolo diedrale non supera la soglia
            int nf = FF(f, (3 + p - 1) % 3);
            if (nf >= 0) {
                int nfi = FFi(f, (3 + p - 1) % 3);
                // prima le facce in senso antiorario
                do {
                    if (FF_cosines(nf, nfi) >= cos_thr) {
                        N.row(3 * nf + nfi) += n;
                        int nnf = FF(nf, (3 + nfi - 1) % 3);
                        nfi = FFi(nf, (3 + nfi - 1) % 3);
                        nf = nnf;
                    } else {
                        break;
                    }
                } while (nf >= 0 && nf != f);

                // poi le facce in senso orario, eventualmente
                if (nf != f) {
                    nf = f;
                    nfi = p;
                    while (nf >= 0 && FF_cosines(nf, nfi) >= cos_thr) {
                        int nnf = FF(nf, nfi);
                        nfi = (FFi(nf, nfi) + 1) % 3;
                        nf = nnf;
                        N.row(3 * nf + nfi) += n;
                    }
                }
            }
        }
    }

    for (int n = 0; n < N.rows(); ++n) {
        N.row(n).normalize();
    }

    return N;
}


//============================================================================
// Name        : jacobian-matrix.cpp
// Author      : Deborah Digges
// Version     :
// Copyright   : 2016
// Description : Hello World in C++, Ansi-style
//============================================================================

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

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;

MatrixXd CalculateJacobian(const VectorXd& x_state);

int main() {

	/*
	 * Compute the Jacobian Matrix
	 */

	//predicted state  example
	//px = 1, py = 2, vx = 0.2, vy = 0.4
	VectorXd x_predicted(4);
	x_predicted << 1, 2, 0.2, 0.4;

	MatrixXd Hj = CalculateJacobian(x_predicted);

	cout << "Hj:" << endl << Hj << endl;

	return 0;
}

MatrixXd CalculateJacobian(const VectorXd& x_state) {

	MatrixXd Hj(3,4);
	//recover state parameters
	float px = x_state(0);
	float py = x_state(1);
	float vx = x_state(2);
	float vy = x_state(3);

	if(px == 0 && py == 0) {
		return Hj;
	}

	float px2py2 = pow(px, 2) + pow(py, 2);

	Hj(0, 0) = px/sqrt(px2py2);
	Hj(0, 1) = py/sqrt(px2py2);

	Hj(1, 0) = -py/px2py2;
	Hj(1, 1) = px/px2py2;

	Hj(2, 0) = py* (vx*py - vy*px)/pow(px2py2, 3/2);
	Hj(2, 1) = px * (vy*px - vx*py)/pow(px2py2, 3/2);
	Hj(2, 2) = px/sqrt(px2py2);
	Hj(2, 3) = py/sqrt(px2py2);

	return Hj;
}


#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>

/**
 * Get most significant byte of n.
 * @param n
 * @return only the most significant byte of n set
 */
uint64_t msb(uint64_t n) {
    if (n == 0)
        return 0;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    return mask;
}

/**
 * Algorithm by Marc Joye and Jean-Jacques Quisquater in Efficient computation of full Lucas sequences.
 */
NTL::ZZ fibonacci1(uint64_t n) {
    if (n == 0)
        return (NTL::ZZ) 0;

    NTL::ZZ Uh;
    Uh = 1;
    NTL::ZZ Vl;
    Vl = 2;
    NTL::ZZ Vh;
    Vh = 1;
    NTL::ZZ Ql;
    Ql = 1;
    NTL::ZZ Qh;
    Qh = 1;

    uint64_t s = 0;
    while (n % 2 == 0) {
        n /= 2;
        s++;
    }

    uint64_t mask = msb(n);
    while (mask != 1) {
        Ql = Ql * Qh;
        if (n & mask) {
            Qh = -Ql;
            Uh = Uh * Vh;
            Vl = Vh * Vl - Ql;
            Vh = Vh * Vh - 2*Qh; // mistake in the original paper
        }
        else {
            Qh = Ql;
            Uh = Uh * Vl - Ql;
            Vh = Vh * Vl - Ql;
            Vl = Vl * Vl - 2 * Ql;
        }

        mask >>= (uint) 1;
    }

    Ql = Ql * Qh;
    Qh = -Ql;
    Uh = Uh * Vl - Ql;
    Vl = Vh * Vl - Ql;
    Ql = Ql * Qh;

    for (uint64_t i = 1; i <= s; i++) {
        Uh = Uh * Vl;
        Vl = Vl * Vl - 2*Ql;
        Ql = Ql * Ql;
    }

    return Uh;
}

/**
 * Algorithm by Aleksey Koval in On Lucas Sequences Computation.
 */
NTL::ZZ fibonacci2(uint64_t n) {
    NTL::ZZ V_l;
    V_l = 2;
    NTL::ZZ V_h;
    V_h = 1;

    NTL::ZZ Q_l;
    Q_l = 1;
    NTL::ZZ Q_h;
    Q_h = 1;

    uint64_t mask = msb(n);
    while (mask != 0) {
        Q_l = Q_l * Q_h;
        if (n & mask) {
            Q_h = -Q_l;
            V_l = V_h * V_l - Q_l;
            V_h = V_h * V_h - 2 * Q_h;
        }
        else {
            Q_h = Q_l;
            V_h = V_h * V_l - Q_l;
            V_l = V_l * V_l - 2 * Q_h;
        }

        mask >>= (uint) 1;
    }

    return (2 * V_h - V_l) / 5;
}

/**
 * Algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci3(uint64_t n) {
    NTL::ZZ x;
    x = 0;
    NTL::ZZ y;
    y = 1;

    NTL::ZZ tmp;

    uint64_t mask = msb(n);
    while (mask != 0) {
        tmp = x;
        x = x*(2*y-x);
        y = (tmp * tmp + y * y);

        if (n & mask) {
            tmp = x;
            x = y;
            y = (tmp + y);
        }

        mask >>= (uint) 1;
    }

    return x;
}

/**
 * Adjusted algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci4(uint64_t n) {
    NTL::ZZ x;
    x = 0;
    NTL::ZZ y;
    y = 1;

    NTL::ZZ x2;
    NTL::ZZ y2;

    uint64_t mask = msb(n);
    while (mask != 0) {
        x2 = x*x;
        y2 = y*y;

        if (n & mask) {
            y = 2*x*y + y2;
            x = x2+y2;
        }
        else {
            x = 2*x*y - x2;
            y = x2+y2;
        }

        mask >>= (uint) 1;
    }

    return x;
}

/**
 * Modular version of algorithm by Marc Joye and Jean-Jacques Quisquater in Efficient computation of full Lucas sequences.
 */
NTL::ZZ fibonacci_mod1(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    if (n == 0)
        return (NTL::ZZ) 0;

    NTL::ZZ_p Uh;
    Uh = 1;
    NTL::ZZ_p Vl;
    Vl = 2;
    NTL::ZZ_p Vh;
    Vh = 1;
    NTL::ZZ_p Ql;
    Ql = 1;
    NTL::ZZ_p Qh;
    Qh = 1;

    uint64_t s = 0;
    while (n % 2 == 0) {
        n /= 2;
        s++;
    }

    uint64_t mask = msb(n);
    while (mask != 1) {
        Ql = Ql * Qh;
        if (n & mask) {
            Qh = -Ql;
            Uh = Uh * Vh;
            Vl = Vh * Vl - Ql;
            Vh = Vh * Vh - 2*Qh; // mistake in the original paper
        }
        else {
            Qh = Ql;
            Uh = Uh * Vl - Ql;
            Vh = Vh * Vl - Ql;
            Vl = Vl * Vl - 2 * Ql;
        }

        mask >>= (uint) 1;
    }

    Ql = Ql * Qh;
    Qh = -Ql;
    Uh = Uh * Vl - Ql;
    Vl = Vh * Vl - Ql;
    Ql = Ql * Qh;

    for (uint64_t i = 1; i <= s; i++) {
        Uh = Uh * Vl;
        Vl = Vl * Vl - 2*Ql;
        Ql = Ql * Ql;
    }

    return rep(Uh);
}

/**
 * Modular version of algorithm by Aleksey Koval in On Lucas Sequences Computation.
 */
NTL::ZZ fibonacci_mod2(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    NTL::ZZ_p V_l;
    V_l = 2;
    NTL::ZZ_p V_h;
    V_h = 1;

    NTL::ZZ_p Q_l;
    Q_l = 1;
    NTL::ZZ_p Q_h;
    Q_h = 1;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    while (mask != 0) {
        Q_l = Q_l * Q_h;
        if (n & mask) {
            Q_h = -Q_l;
            V_l = V_h * V_l - Q_l;
            V_h = V_h * V_h - 2 * Q_h;
        }
        else {
            Q_h = Q_l;
            V_h = V_h * V_l - Q_l;
            V_l = V_l * V_l - 2 * Q_h;
        }

        mask >>= (uint) 1;
    }

    return rep((2 * V_h - V_l) / 5);  // fails if 5 divides m
}

/**
 * Modular version of algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci_mod3(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    NTL::ZZ_p x;
    x = 0;
    NTL::ZZ_p y;
    y = 1;

    NTL::ZZ_p tmp;

    uint64_t mask = msb(n);
    while (mask != 0) {
        tmp = x;
        x = x*(2*y-x);
        y = (tmp * tmp + y * y);

        if (n & mask) {
            tmp = x;
            x = y;
            y = (tmp + y);
        }

        mask >>= (uint) 1;
    }

    return rep(x);
}

/**
 * Modular version of adjusted algorithm derived from matrix multiplication.
 */
NTL::ZZ fibonacci_mod4(uint64_t n, uint64_t m) {
    NTL::ZZ_p::init((NTL::ZZ) m);

    NTL::ZZ_p x;
    x = 0;
    NTL::ZZ_p y;
    y = 1;

    uint64_t tmp;
    tmp = n >> (uint) 1;

    uint64_t mask = 1;
    while (tmp != 0) {
        tmp >>= (uint) 1;
        mask <<= (uint) 1;
    }

    NTL::ZZ_p x2;
    NTL::ZZ_p y2;

    while (mask != 0) {
        x2 = x*x;
        y2 = y*y;

        if (n & mask) {
            y = 2*x*y + y2;
            x = x2+y2;
        }
        else {
            x = 2*x*y - x2;
            y = x2+y2;
        }

        mask >>= (uint) 1;
    }

    return rep(x);
}


#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <map>
#include <utility>
#include <vector>
// this header for all except for ch8
// MIT LICENSE
#ifndef STATISTICS_HPP
#define STATISTICS_HPP
using namespace std;
#define yes true
#define no false

double genPValue(double z_value, bool isTwoTail = false, bool tailInvertSign = false) {
    // need to add two tail test!
    // add bool isTwotail = false
    // this is the conjugate function of genZValue
    boost::math::normal Ndistribution(0, 1);
    auto P = boost::math::cdf(boost::math::complement(Ndistribution, fabs(z_value)));
    if (isTwoTail && tailInvertSign)
        throw "error range";
    if (tailInvertSign)
        P = 1 - P;
    if (isTwoTail)
        P *= 2;
    return P;
}

double genPValue(double degree, double t_value, bool isTwoTail = false, bool tailInvertSign = false) {
    // this is the conjugate function of genTValue
    // @degree: the degree of freedom
    boost::math::students_t Tdistribution(degree);
    auto P = boost::math::cdf(boost::math::complement(Tdistribution, fabs(t_value)));
    if (isTwoTail && tailInvertSign)
        throw "error range";
    if (tailInvertSign)
        P = 1 - P;
    if (isTwoTail)
        P *= 2;
    return P;
}

double genZValue(double subscript, bool isSingleTail) {
    /*
    * @subscript: the score below the "z", which means the cumulative distribution function of the area.
    *             if subscript is negative meaning it is left tail, possitive otherwise
    * @isSingleTail: is only sigle tail of the reject area
    * return the z-score which lookup from the z-table
    */
    boost::math::normal Ndistribution(0, 1);
    if (!isSingleTail)
        subscript /= 2;
    auto Z = boost::math::quantile(boost::math::complement(Ndistribution, fabs(subscript)));
    if (subscript < 0)
        Z = -Z;
    return Z;
}

double genTValue(int degree, double upperTailArea) {
    boost::math::students_t Tdistribution(degree);
    auto T = boost::math::quantile(boost::math::complement(Tdistribution, upperTailArea));
    return T;
}

template <typename Iteratable>
double genMean(Iteratable& _dataSet) {
    double sum = 0;
    for (auto i : _dataSet)
        sum += i;
    return sum / _dataSet.size();
}

template <typename Iteratable>
double genSampleStandardDeviation(Iteratable& _dataSet, double sampleMean, double size) {
    double sxx = 0;
    for (auto i : _dataSet)
        sxx += (i - sampleMean) * (i - sampleMean);
    return sqrt(1 / (size - 1) * sxx);
}

double genPercentageStandardDeviation(double p, int sampleSize) {
    return sqrt(p * (1 - p) / sampleSize);
}

int twoPopulationDegreeFreedom(double ssd1, double ssd2, int n1, int n2) {
    auto sampleVariation1 = (ssd1 * ssd1 / n1);
    auto sampleVariation2 = (ssd2 * ssd2 / n2);
    return (sampleVariation1 + sampleVariation2) * (sampleVariation1 + sampleVariation2) /
           (sampleVariation1 * sampleVariation1 / (n1 - 1) + sampleVariation2 * sampleVariation2 / (n2 - 1));
}
template <typename Iteratable>
int twoPopulationDegreeFreedom(Iteratable& _dataSet1, Iteratable& _dataSet2) {
    auto ssd1 = genSampleStandardDeviation(_dataSet1, genMean(_dataSet1), _dataSet1.size());
    auto ssd2 = genSampleStandardDeviation(_dataSet2, genMean(_dataSet2), _dataSet2.size());
    return twoPopulationDegreeFreedom(ssd1, ssd2, _dataSet1.size(), _dataSet2.size());
}

double errorRadius(vector<double>& _dataSet, int degree, int sampleSize, double upperTailArea) {
    double mean = genMean(_dataSet);
    double ssd = genSampleStandardDeviation(_dataSet, mean, sampleSize);
    return ssd / sqrt(sampleSize) * genTValue(degree, upperTailArea);
}

double errorRadius(vector<double>& _dataSet, double upperTailArea = 0.025) {
    /*
    * giving data set and the upper tail area of Student-T distribution
    * return the error radius
    * for too lazy to only input data set
    */
    int sampleSize = _dataSet.size();
    return errorRadius(_dataSet, sampleSize - 1, sampleSize, upperTailArea);
}

double errorRadius(double knownSigma, double alpha, int sampleSize, bool isSingleTail = false, bool isSample = false) {
    /*
    * @knownSigma: the population sigma, which over sqrt(n)
    * giving alpha return the error radius for known Sigma of Z distribution
    * return the error radius
    */

    if (isSample) {
        if (!isSingleTail)
            alpha /= 2;
        return knownSigma / sqrt(sampleSize) * genTValue(sampleSize - 1, alpha);
    }
    return knownSigma / sqrt(sampleSize) * genZValue(alpha, isSingleTail);
}

pair<double, double> genConfidenceInterval(double theta, double errorRadius, int tailOrient = 0) {
    /*
     * @theta: the sample variable
     * @errorRadius: a radius of error, which might greater than 1,
     *               means the standardDeviation times (z or t) over sqrt(n)
     * @tailOrient: if (tailOrient > 0) will reject right tail
     *              else if (tailOrient < 0) will reject left tail
     *              else will reject two tail
     */
    if (tailOrient > 0)
        return make_pair(-numeric_limits<double>::infinity(), theta + errorRadius);
    else if (tailOrient < 0)
        return make_pair(theta - errorRadius, numeric_limits<double>::infinity());
    else
        return make_pair(theta - errorRadius, theta + errorRadius);
}

vector<pair<double, double>> invertInterval(pair<double, double> originInterval) {
    if (originInterval.first == -numeric_limits<double>::infinity())
        return {make_pair(originInterval.second, numeric_limits<double>::infinity())};
    else if (originInterval.second == numeric_limits<double>::infinity())
        return {make_pair(-numeric_limits<double>::infinity(), originInterval.first)};
    else
        return {make_pair(-numeric_limits<double>::infinity(), originInterval.first),
                make_pair(originInterval.second, numeric_limits<double>::infinity())};
}

pair<double, double> standardlizeInterval(pair<double, double> originInterval, double mu, double standardDeviation) {
    /**
     * @originInterval: the interval need to be standardlized
     * @mu: the mean
     * @standardDeviation: the standard deviation of mu
     */
    return make_pair((originInterval.first - mu) / standardDeviation, (originInterval.second - mu) / standardDeviation);
}

double intervalProbability(pair<double, double> standardlizedInterval) {
    /*
    * this will return the probability with z distribution in some interval
    * @standardlizedInterval: the standardlized interval which want to get the probability
    */
    double leftTail = genPValue(standardlizedInterval.first, false, false),
           rightTail = genPValue(standardlizedInterval.second, false, false);
    return 1 - leftTail - rightTail;
}

template <typename T, typename S>
ostream& operator<<(ostream& os, const pair<T, S>& v) {
    pair<char, char> boundSign('[', ']');
    if (v.first == -numeric_limits<double>::infinity())
        boundSign.first = '(';
    if (v.second == numeric_limits<double>::infinity())
        boundSign.second = ')';
    os << boundSign.first << v.first << ", " << v.second << boundSign.second;
    return os;
}

namespace needingSampleSize {
int p(double alpha, double p = 0.5, double marginError = 0.95) {
    /*
    * @p: the p bar of sample proportions, which might be iid Ber(P) (Bernoulli distribution)
    * @marginError: the margin error of confidence interval
    */
    double q = 1 - p, z = genZValue(alpha, false);
    auto sampleSize = p * q * z * z / marginError / marginError;
    return ceil(sampleSize);
}

int x_bar(double alpha, double knownTheta, double marginError) {
    /*
    * @knownTheta: the sigma which is the population standard deviation
    * @marginError: the margin error of confidence interval
    */
    double z = genZValue(alpha, false);
    auto sampleSize = z * z * knownTheta * knownTheta / marginError / marginError;
    return ceil(sampleSize);
}
int hypothesis(double mu_0, double mu_a, double za, double zb, double sigma) {
    return ceil(pow((za + zb) * sigma / (mu_0 - mu_a), 2));
}
}  // namespace needingSampleSize

double testStatistic(double x_bar, double mu_0, double sd, int sampleSize, bool isSample = false) {
    /**
     * @x_bar: sampleMean
     * @sd: standard deviation
     * @mu_0: the null hypotheses
     * @sampleSize: the sample size which want to againest the H0
     * @isSample: Are we not know the population @sd?
     *          if(false): we don't know the population standard deviation
     */
    if (isSample)
        sampleSize -= 1;
    return (x_bar - mu_0) / (sd / sqrt(sampleSize));
}

template <class T>
string readSingleLineCSV(vector<T>& _dataSet, string fileName) {
    /* will return the file's title
    * read only single line csv file
    * @_dataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawdata;
    getline(inFile, title);
    while (inFile >> rawdata)
        _dataSet.push_back(rawdata);
    inFile.close();
    return title;
}

template <class T>
string readSingleLineCSV(map<T, int>& proportionDataSet, string fileName) {
    /*
    * only for reading proportion Data
    * will return the file's title
    * read only single line csv file
    * @proportionDataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawData;
    getline(inFile, title);
    while (getline(inFile, rawData)) {
        auto isInMap = proportionDataSet.find(rawData);
        if (isInMap != proportionDataSet.end())
            proportionDataSet.at(isInMap->first)++;
        else
            proportionDataSet.insert(pair<T, int>(rawData, 1));
    }
    inFile.close();
    return title;
}

string readMultiLineCSV(map<string, map<string, int>>& proportionDataSet, string fileName) {
    // @proportionDataSet: {title, {data, freq}}
    // return file name
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string titles, singleLine;
    vector<string> titlesContainer;
    getline(inFile, titles);
    for (auto index = titles.find(','); index != string::npos; index = titles.find(',')) {
        string firstSubstring(titles.substr(0, index));
        titlesContainer.push_back(firstSubstring);
        titles.erase(0, firstSubstring.length() + 1);
    }
    titlesContainer.push_back(titles);  //push last title

    while (getline(inFile, singleLine)) {
        for (auto i : titlesContainer) {
            auto index = singleLine.find(',');
            string tmp;
            if (index != string::npos)
                tmp = singleLine.substr(0, index);
            else
                tmp = singleLine;  //the last element

            if (tmp.length()) {
                auto isTitleInTable = proportionDataSet.find(i);
                if (isTitleInTable != proportionDataSet.end()) {
                    auto isInMap = isTitleInTable->second.find(tmp);
                    if (isInMap != isTitleInTable->second.end())
                        isTitleInTable->second.at(isInMap->first)++;
                    else
                        isTitleInTable->second.insert(make_pair(tmp, 1));
                } else {
                    auto inMap = make_pair(tmp, 1);
                    proportionDataSet[i].insert(inMap);
                }
            }
            singleLine.erase(0, tmp.length() + 1);
        }
    }
    inFile.close();
    return fileName;
}
#endif


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  // Always normalize quaternions before using them
  q1.normalize();
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  // Note here T1w is the transform from the world to frame 1
  // This will transform a point from world coordinate frame to frame 1
  // Robot poses generally mean the opposite
  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;
  return 0;
}