#pragma once
// kalman.hpp
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-BLAS project, which is released under an MIT Open Source license.
#include <boost/numeric/mtl/mtl.hpp>

template<typename Ty>
class KalmanFilter {
public:

	/**
	* Create a Kalman filter with the specified matrices.
	*   A - System dynamics matrix
	*   C - Output matrix
	*   Q - Process noise covariance
	*   R - Measurement noise covariance
	*   P - Estimate error covariance
	*/
	KalmanFilter(
		double dt,
		const mtl::dense2D<Ty>& A,
		const mtl::dense2D<Ty>& C,
		const mtl::dense2D<Ty>& Q,
		const mtl::dense2D<Ty>& R,
		const mtl::dense2D<Ty>& P
	) : A(A), C(C), Q(Q), R(R), P0(P),
		m(num_rows(C)), n(num_rows(A)), dt(dt), initialized(false),
		I(n, n), x_hat(n), t(0), t0(0)
	{
		I = mtl::mat::identity2D(m,n); // I.setIdentity();
	}
	KalmanFilter() {}

	/**
	* Initialize the filter with initial states as zero.
	*/
	void init() {
		x_hat.setToZero();
		P = P0;
		t0 = 0;
		t = t0;
	}

	/**
	* Initialize the filter with a guess for initial states.
	*/
	void init(double _t0, const mtl::dense_vector<Ty>& _x0) {
		x_hat = _x0;
		P = P0;
		t0 = _t0;
		t = t0;
	}

	/**
	* Update the estimated state based on measured values. The
	* time step is assumed to remain constant.
	*/
	void update(const mtl::dense_vector<Ty>& y) {
		mtl::dense_vector<Ty> x_hat_new(n);
		x_hat_new = A * x_hat;
//		P = A * P * trans(A) + Q;
//		K = P * trans(C) * (C * P * trans(C) + R).inverse();
//		x_hat_new += K * (y - C * x_hat_new);
//		P = (I - K * C) * P;
		x_hat = x_hat_new;
	}

	/**
	* Update the estimated state based on measured values,
	* using the given time step and dynamics matrix.
	*/
	void update(const mtl::dense_vector<Ty>& _y, double _dt, const mtl::dense2D<Ty> _A) {
		A = _A;
		dt = _dt;
		update(_y);
	}

	/**
	* Return the current state and time.
	*/
	mtl::dense_vector<Ty> state() { return x_hat; };
	double time() { return t; };

private:
	bool initialized;

	// Matrices for computation
	mtl::dense2D<Ty> A, C, Q, R, P, K, P0;

	// System dimensions
	size_t m, n;

	// Initial and current time
	double t0, t;

	// Discrete time step
	double dt;

	// n-size identity
	mtl::dense2D<Ty> I;

	// Estimated states
	mtl::dense_vector<Ty> x_hat;
};


#pragma once

#include <Eigen/Geometry>

#include <autodiff/autodiff.h>
#include <interval/interval.hpp>
#include <utils/eigen_ext.hpp>

namespace ipc::rigid {

double sinc(const double& x);

Interval sinc(const Interval& x);

/// Compute the L2 norm (∑|xᵢ|²)
template <typename T> T norm(const VectorMax3<T>& x)
{
    // Do an explicit abs to avoid possible problems with intervals
    VectorMax3<T> absx(x.size());
    for (int i = 0; i < x.size(); i++) {
        absx(i) = abs(x(i));
    }
    return sqrt(absx.dot(absx));
}

/// Compute sinc(||x||) for x ∈ Rⁿ
template <typename T> T sinc_normx(const VectorMax3<T>& x)
{
    static_assert(
        !std::is_base_of<DiffScalarBase, T>::value,
        "This version does not work with autodiff!");
    return sinc(norm(x));
}

/// Compute ∇sinc(||x||) for x ∈ Rⁿ
VectorMax3d sinc_normx_grad(const VectorMax3d& x);

/// Compute ∇²sinc(||x||) for x ∈ Rⁿ
MatrixMax3d sinc_normx_hess(const VectorMax3d& x);

template <typename Scalar, typename Gradient>
DScalar1<Scalar, Gradient>
sinc_normx(const VectorMax3<DScalar1<Scalar, Gradient>>& x)
{
    const int m = DiffScalarBase::getVariableCount(), n = x.size();

    // Extract the vector of values
    VectorMax3<Scalar> x_vals(n);
    for (int i = 0; i < n; i++) {
        x_vals(i) = x(i).getValue();
    }

    // Compute the value and gradient (without chain-rule)
    Scalar value = sinc_normx(x_vals);
    VectorMax3d grad = sinc_normx_grad(x_vals);

    // Apply chain-rule
    Gradient full_grad = Gradient::Zero(m);
    for (int j = 0; j < n; j++) {
        full_grad += grad(j) * x(j).getGradient();
    }

    return DScalar1<Scalar, Gradient>(value, full_grad);
}

template <typename Scalar, typename Gradient, typename Hessian>
DScalar2<Scalar, Gradient, Hessian>
sinc_normx(const VectorMax3<DScalar2<Scalar, Gradient, Hessian>>& x)
{
    const int m = DiffScalarBase::getVariableCount(), n = x.size();

    // Extract the vector of values
    VectorMax3<Scalar> x_vals(n);
    for (int i = 0; i < n; i++) {
        x_vals(i) = x(i).getValue();
    }

    // Compute the value, gradient, and hessian (without chain-rule)
    Scalar value = sinc_normx(x_vals);
    VectorMax3d grad = sinc_normx_grad(x_vals);
    MatrixMax3d hess = sinc_normx_hess(x_vals);

    // Apply chain-rule
    Gradient full_grad = Gradient::Zero(m);
    for (int k = 0; k < n; k++) {
        full_grad += grad(k) * x(k).getGradient();
    }

    Hessian full_hess = Hessian::Zero(m, m);
    for (int k = 0; k < n; k++) {
        for (int l = 0; l < n; l++) {
            full_hess += hess(k, l) * x(l).getGradient()
                * x(k).getGradient().transpose();
        }
        full_hess += grad(k) * x(k).getHessian();
    }

    return DScalar2<Scalar, Gradient, Hessian>(value, full_grad, full_hess);
}

} // namespace ipc::rigid


#ifndef TVMTL_MANIFOLD_SON_HPP
#define TVMTL_MANIFOLD_SON_HPP

#include <cmath>
#include <complex>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

#include "enumerators.hpp"
#include "matrix_utils.hpp"

namespace tvmtl {

// Specialization SO(N)
template <int N>
struct Manifold< SO, N> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	//typedef double scalar_type;
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, N>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix <scalar_type, N * N, N * (N - 1) / 2> tm_base_type;
	typedef tm_base_type& tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N*N, N*N>				deriv2_type;
	typedef deriv2_type&							deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N * (N - 1) / 2, N * (N - 1) / 2>	restricted_deriv2_type;

	// Helper Types
	typedef Eigen::PermutationMatrix<N*N, N*N, int> perm_type;

	inline static perm_type ConstructPermutationMatrix();

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	static const perm_type permutation_matrix;

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projection
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x) {};
	inline static void interpolation_postprocessing(ref_type x) {};

};

/*-----IMPLEMENTATION SO----------*/

// Static constants, Outside definition to avoid linker error

template <int N>
const MANIFOLD_TYPE Manifold < SO, N>::MyType = SO;

template <int N>
const int Manifold < SO, N>::manifold_dim = N * (N - 1) / 2;

template <int N>
const int Manifold < SO, N>::value_dim = N * N;

template <int N>
const bool Manifold < SO, N>::non_isometric_embedding = false;

// PermutationMatrix
template <int N>
typename Manifold < SO, N>::perm_type Manifold<SO, N>::ConstructPermutationMatrix(){
    perm_type P;
    P.setIdentity();
    for(int i=0; i<N; i++)
	for(int j=0; j<i; j++)
	    P.applyTranspositionOnTheRight(j*N + i, i*N + j);
    return P;
}

template <int N>
const typename Manifold < SO, N>::perm_type Manifold<SO, N>::permutation_matrix = ConstructPermutationMatrix();

// Squared SO distance function
template <int N>
inline typename Manifold < SO, N>::dist_type Manifold < SO, N>::dist_squared( cref_type x, cref_type y ){
    #ifdef TV_SON_DEBUG
	std::cout << "\nDist2 function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
    return (x.transpose() * y).log().squaredNorm();
}

// Derivative of Squared SO distance w.r.t. first argument
template <int N>
inline void Manifold < SO, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = -2.0 * x * (x.transpose() * y).log();
}
// Derivative of Squared SO distance w.r.t. second argument
template <int N>
inline void Manifold < SO, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result =  -2.0 * y * (y.transpose() * x).log();
}

// Second Derivative of Squared SO distance w.r.t first argument
template <int N>
inline void Manifold < SO, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type XtY = x.transpose()*y;

    deriv2_type logXtY_kron_I ,I_kron_x, Yt_kron_I, dlog;
    logXtY_kron_I = Eigen::kroneckerProduct(XtY.log().eval(),value_type::Identity());
    I_kron_x = Eigen::kroneckerProduct(value_type::Identity(),x);
    Yt_kron_I = Eigen::kroneckerProduct(y.transpose(),value_type::Identity());
    KroneckerDLog(XtY, dlog);

    result = -2.0 * (logXtY_kron_I.transpose() + I_kron_x * dlog * Yt_kron_I * permutation_matrix );
}
// Second Derivative of Squared SO distance w.r.t first and second argument
template <int N>
inline void Manifold < SO, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type XtY = x.transpose()*y;

    deriv2_type I_kron_x, dlog;
    I_kron_x = Eigen::kroneckerProduct(value_type::Identity(),x);
    KroneckerDLog(XtY, dlog);

    result = -2.0 * I_kron_x * dlog * I_kron_x.transpose();
}
// Second Derivative of Squared SO distance w.r.t second argument
template <int N>
inline void Manifold < SO, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2xx_dist_squared(y, x, result);
}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY>
inline void Manifold <SO, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    result = x * (x.transpose() * y).exp();
}

template <int N>
inline void Manifold <SO, N>::log(cref_type x, cref_type y, ref_type result){
    result = x * (x.transpose() * y).log();
}

// Tangent Plane restriction
template <int N>
inline void Manifold <SO, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    int d = value_type::RowsAtCompileTime;
    int k = 0;

    value_type T;

    for(int i=0; i<d-1; i++)
	for(int j=i+1; j<d; j++){
	    T.setZero();
	    scalar_type sqrt = 1.0/std::sqrt(2);
	    T.col(i) = -sqrt * x.col(j);
	    T.col(j) =  sqrt * x.col(i);

	    result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	    k++;
	}
}

// Convex geodesic combinations
template <int N>
inline void Manifold <SO, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    value_type l;
    if (t == 0.5){
	result = x + y;
	projector(result);
    }
    else{
    log(x, y, l);
    exp(x, l * t, result);
    }
}

// Karcher mean implementations
template <int N>
inline void Manifold<SO, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 1.0 / v.size() * L , temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
inline void Manifold<SO, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 1.0 / v.size() * L , temp);
	x = temp;
	projector(x);
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
template <typename V, class... Args>
inline void Manifold<SO, N>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 1.0 / numArgs * sum, temp);
	x = temp;
	projector(x);
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N>
template <typename V>
inline void Manifold<SO, N>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N>
template <typename V, class... Args>
inline void Manifold<SO, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N>
inline void Manifold <SO, N>::projector(ref_type x){

    #ifdef TV_SON_DEBUG
	std::cout << "\n\nProjector with initial x=\n" << x << std::endl;
    #endif

    Eigen::JacobiSVD<value_type> svd(x, Eigen::ComputeThinU | Eigen::ComputeThinV);
    x = svd.matrixU() * svd.matrixV().transpose();
    //if(x.determinant() < 0)
    //	x.row(1)*=-1.0;

    #ifdef TV_SON_DEBUG
	std::cout << "\nProjector with final x=\n" << x << std::endl;
    #endif
}

} // end namespace tvmtl

#endif


/*****************************************************************************
*
* Copyright (C) 2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iomanip>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

int main() {
  using std::abs; using std::sqrt;
  typedef double real_t;

  auto f = [](real_t x) { return sqrt(1 - x * x); };
  real_t r = boost::math::constants::pi<real_t>() / 2;

  boost::math::quadrature::tanh_sinh<real_t> integrator;
  real_t termination = sqrt(std::numeric_limits<real_t>::epsilon());
  real_t error, L1;
  size_t levels;
  real_t q = integrator.integrate(f, -1, 1, termination, &error, &L1, &levels);

  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "result: " << q << std::endl
            << "estimated error: " << error << std::endl
            << "real error: " << abs(q - r) << std::endl
            << "L1 * error: " << L1 * error << std::endl
            << "levels: " << levels << std::endl;
}


#include <boost/program_options.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <cmath>
#include <stdlib.h>
#include <stdint.h>
#include <random>
#include <algorithm>    // std::min_element, std::max_element

#include "structures.h"
#include "clear_ciffers.h"

// Questions 2 and 3
// Display the linear approximation matrix.
void linearApproximationMatrix(int nbElt=16) {
    std::vector<std::pair<int,int>> goodKeys ;
    std::cout << "\\[\n\\bordermatrix{\n& " ;
    for(int b = 0 ; b < nbElt-1 ; b++) {
        std::cout << b << " & " ;
    }
    std::cout << nbElt-1 << "\\cr\n" ;
    for(int a = 0 ; a < nbElt ; a++) {
        std::cout << a << " & " ;
        for(int b = 0 ; b < nbElt ; b++) {
            int nbKey = 0 ;
            for(int c = 0 ; c < nbElt ; c++) {
                Block blockA(a) ;
                Block blockB(b) ;
                Block key(c) ;
                blockA.product(key) ;
                key.substitution() ;
                blockB.product(key) ;
                if(blockA.bitsXor() == blockB.bitsXor()) {
                    nbKey ++ ;
                }
            }
            std::cout << nbKey ;
            double proba = ((double)nbKey)/16 ;
            if(std::abs(proba - 0.5) > 0.3) {
                // std::cout << nbKey ;
                goodKeys.push_back(std::pair<int,int>(a, b)) ;
            }
            if(b < nbElt-1)
                std::cout << " & " ;
        }
        std::cout << "\\cr\n" ;
    }
    std::cout << "}\n\\]\n";
    std::cout << "Couples (a, b) with the highest probability:\n\\[" ;
    if(goodKeys.size() > 0)
        std::cout << "(" << goodKeys[0].first << ", " << goodKeys[0].second << ")" ;
    for(unsigned i = 1 ; i < goodKeys.size() ; i++) {
        std::cout << ", (" << goodKeys[i].first << ", " << goodKeys[i].second << ")" ;
    }
    std::cout << "\\]\n" ;
}

// Question 4
// Check experimentaly that Prob(a.m = P(b).c) is in {1/2-1/8, 1/2+1/8}
void experimentalCheck(uint32_t a=1, uint32_t b=5) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<unsigned int> dis(0, UINT32_MAX);
    Block A(a<<28);
    Block B(b<<28);
    B.permutation() ;
    // int nbMess = 1000000 ;
    int nbMess = 100000 ;
    for(int i = 0 ; i < 10 ; i++) {
        Block K0(dis(gen)) ;
        Block K1(dis(gen)) ;
        int nbEqual = 0 ;
        for(int j = 0 ; j < nbMess ; j++) {
            Block m(dis(gen)) ;
            Block x(m) ;
            x.addition(K0) ;
            x.turn(K1) ;
            m.product(A) ;
            x.product(B) ;
            if(m.bitsXor() == x.bitsXor())
                nbEqual ++ ;
        }
        std::cout << "& $" << (double)nbEqual/(double)nbMess << "$ ";//std::endl ;
    }
    std::cout << "\\\\" << std::endl ;
}

// Questions 7-8
// Guess the block of given index of the key K2.
int guessKeyBox(int blockIndex, uint32_t a=4, uint32_t b=8) {
    std::vector<int> keyCount(16, 0) ;
    Block A(a<<(blockIndex*4)) ;
    Block B(b<<(blockIndex*4)) ;
    B.permutation(2) ;
    // Compute the distribution of each key over all the couples plaintext/ciphertext
    for(unsigned int i = 0 ; i < Plaintext.size() ; i++) {
        Block M(Plaintext[i]) ;
        Block C(Ciphertext[i]) ;
        C.permutation(-2) ;
        for(uint32_t k = 0 ; k < 16 ; k++) {
            Block K(k<<(blockIndex*4)) ;
            K.addition(C) ;
            K.substitution(REVERSE_DEFAULT_SUBST) ;
            Block Mcopy(M) ;
            Mcopy.product(A) ;
            K.product(B) ;
            if(Mcopy.bitsXor() == K.bitsXor())
                keyCount[k] ++ ;
        }
    }
    // The key with the distribution the farthest from 1/2 is probably the key that we are looking for.
    int kmin = std::min_element(keyCount.begin(), keyCount.end()) - keyCount.begin();
    int kmax = std::max_element(keyCount.begin(), keyCount.end()) - keyCount.begin();
    if(std::abs((double)Plaintext.size()/2 - keyCount[kmin]) > std::abs((double)Plaintext.size()/2 - keyCount[kmax]))
        return kmin ;
    else
        return kmax ;
}

// Questions 7-8
// Guess the key K2 block by block.
Block guessK2() {
    Block K(0) ;
    for(int i = 0 ; i < 8 ; i++) {
        K.setBox(i, guessKeyBox(i)) ;
    }
    K.permutation() ;
    return K ;
}

// Check if the given keys are the keys which crypted the table Plaintext.
bool checkSolution(Block K0, Block K1, Block K2) {
    unsigned int i ;
    for(i = 0 ; i < Plaintext.size() ; i++) {
        Block m(Plaintext[i]) ;
        Block c(Ciphertext[i]) ;
        m.encrypt(K0, K1, K2) ;
        if(m.getBits() != c.getBits())
            break ;
    }
    return i == Plaintext.size() ;
}

// Not in the subject
// Retrieve the keys with a brute force attack.
// Too long, not fully tested.
void bruteForce() { // too long
    uint32_t k = 0 ;
    Block K0, K1, K2 ;
    while(1) {
        if(k%10000 == 0)
            std::cout << k << "/" << UINT32_MAX << std::endl ;
        Block K(k) ;
        K.generateSubKeys(&K0, &K1, &K2) ;
        if(checkSolution(K0, K1, K2))
            break ;
        k++ ;
    }
    std::cout << "Found keys by brute force:" << std::endl ;
    std::cout << "K  = " << k << std::endl ;
    std::cout << "K0 = " << K0.getBits() << std::endl ;
    std::cout << "K1 = " << K1.getBits() << std::endl ;
    std::cout << "K2 = " << K2.getBits() << std::endl ;
}

// Question 9
// Find the keys with the method of the actives boxes.
Block guessKey() {
    int bit ;
    std::vector<int> K2Subst = DEFAULT_K2_SUBST ;
    Block K2 = guessK2() ;
    Block K ;
    std::vector<bool> knownBit(32, false) ;
    // Build the key K with the key K2
    for(int i = 0 ; i < 32 ; i++) {
        K.setBox(K2Subst[i], K2.getBox(i, 1), 1) ;
        knownBit[K2Subst[i]] = true ;
    }
    // Bits of K that we do not know
    std::vector<int> unknownPositions ;
    for(int i = 0 ; i < 32 ; i++) {
        if(!knownBit[i])
            unknownPositions.push_back(i) ;
    }
    std::vector<int> bitsToGuess(unknownPositions.size(), 0) ;
    Block K0, K1, K2bis ;
    // Brute force attack on the unknown bits
    K.generateSubKeys(&K0, &K1, &K2bis) ;
    assert(K2bis.getBits() == K2.getBits()) ;
    while(!checkSolution(K0, K1, K2)) {
        int i = bitsToGuess.size() - 1 ;
        while(bitsToGuess[i] == 1) {
            bitsToGuess[i] = 0 ;
            K.setBox(unknownPositions[i], 0, 1) ;
            i-- ;
        }
        bitsToGuess[i] = 1 ;
        K.setBox(unknownPositions[i], 1, 1) ;
        K.generateSubKeys(&K0, &K1, &K2bis) ;
        assert(K2bis.getBits() == K2.getBits()) ;
    }
    return K ;
}

namespace po = boost::program_options;

int main(int argc, char *argv[]) {
    po::options_description description("Usage");

    description.add_options()
        ("help,h", "produce help message")
        ("linearApproximationMatrix", "display the linear approximation matrix")
        ("experimentalCheck", "display the experimental results")
        ("bruteForce", "retrieve the keys by a brute force attack (very long)")
        ("activeBox", "retrieve the keys by active boxes attack") ;
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(description).run(), vm);
    po::notify(vm);

    if(vm.count("help")){
        std::cout << description;
        return 0 ;
    }

    if(vm.count("linearApproximationMatrix")) {
        linearApproximationMatrix() ;
    }

    if(vm.count("experimentalCheck")) {
        std::vector<std::pair<int, int>> couples = {std::pair<int, int>(1, 5),\
            std::pair<int, int>(3, 15), std::pair<int, int>(4, 8), std::pair<int, int>(7, 7),\
            std::pair<int, int>(9, 4), std::pair<int, int>(10, 11), std::pair<int, int>(13, 12)} ;
        for(unsigned i = 0 ; i < couples.size() ; i++) {
            std::cout << "$(" << couples[i].first << ", " << couples[i].second << ")$ " ;
            experimentalCheck(couples[i].first, couples[i].second) ;
        }
    }

    if(vm.count("bruteForce")) {
        bruteForce() ;
    }

    if(vm.count("activeBox")) {
        Block K = guessKey() ;
        Block K0, K1, K2 ;
        K.generateSubKeys(&K0, &K1, &K2) ;
        std::cout << "K  = " << K.getBits() << std::endl ;
        std::cout << "K0 = " << K0.getBits() << std::endl ;
        std::cout << "K1 = " << K1.getBits() << std::endl ;
        std::cout << "K2 = " << K2.getBits() << std::endl ;
    }

    return 0 ;
}


/*
 * MultivariateNormalDistribution.hpp
 *
 *  Created on: Oct 6, 2012
 *      Author: bing
 */

#ifndef MULTIVARIATENORMALDISTRIBUTION_HPP_
#define MULTIVARIATENORMALDISTRIBUTION_HPP_

#include <boost/random.hpp>
#include <Eigen/Cholesky>

template<class T, size_t N>
class MultivariateNormalDistribution {
public:
	MultivariateNormalDistribution(unsigned int seed = time(0));

	MultivariateNormalDistribution(const Eigen::Matrix<T, N, N>& covarianceMatrix, unsigned int seed = time(0));

	virtual ~MultivariateNormalDistribution();

	void setCovarianceMatrix(const Eigen::Matrix<T, N, N>& covarianceMatrix);

	Eigen::Matrix<T, N, 1> draw();

	Eigen::Matrix<T, N, 1> draw(const Eigen::Matrix<T, N, N>& covarianceMatrix);

private:
	Eigen::Matrix<T, N, 1> draw(const Eigen::LLT<Eigen::Matrix<T, N, N> >& llt);

private:
	Eigen::Matrix<T, N, N> covarianceMatrix_;
	Eigen::LLT<Eigen::Matrix<T, N, N> > llt_;
	boost::mt19937 rng_;
};

template<class T, size_t N>
MultivariateNormalDistribution<T, N>::MultivariateNormalDistribution(unsigned int seed) :
		covarianceMatrix_(Eigen::Matrix<T, N, N>::Identity()), llt_(Eigen::Matrix<T, N, N>::Identity()), rng_(seed) {

}

template<class T, size_t N>
MultivariateNormalDistribution<T, N>::MultivariateNormalDistribution(const Eigen::Matrix<T, N, N>& covarianceMatrix, unsigned int seed) :
		covarianceMatrix_(covarianceMatrix), llt_(covarianceMatrix), rng_(seed) {

}

template<class T, size_t N>
MultivariateNormalDistribution<T, N>::~MultivariateNormalDistribution() {

}

template<class T, size_t N>
void MultivariateNormalDistribution<T, N>::setCovarianceMatrix(const Eigen::Matrix<T, N, N>& covarianceMatrix) {
	covarianceMatrix_ = covarianceMatrix;
	llt_ = Eigen::LLT<Eigen::Matrix<T, N, N> >(covarianceMatrix);
}

template<class T, size_t N>
Eigen::Matrix<T, N, 1> MultivariateNormalDistribution<T, N>::draw() {
	return draw(llt_);
}

template<class T, size_t N>
Eigen::Matrix<T, N, 1> MultivariateNormalDistribution<T, N>::draw(const Eigen::Matrix<T, N, N>& covarianceMatrix) {
	return draw(Eigen::LLT<Eigen::Matrix<T, N, N> >(covarianceMatrix));
}

template<class T, size_t N>
Eigen::Matrix<T, N, 1> MultivariateNormalDistribution<T, N>::draw(const Eigen::LLT<Eigen::Matrix<T, N, N> >& llt) {
	// Draw N independent random numbers from standard normal distribution.
	boost::normal_distribution<double> normalDistribution(0.0, 1.0);
	boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > variateGenerator(rng_, normalDistribution);
	Eigen::Matrix<T, N, 1> r(N);
	for (unsigned int i = 0; i < N; i++) {
		r(i) = variateGenerator();
	}

	// Transform numbers to multivariate normal distribution.
	Eigen::Matrix<T, N, 1> z = llt.matrixL() * r;

	return z;
}

#endif /* MULTIVARIATENORMALDISTRIBUTION_HPP_ */


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief Solve the autonomous IVP y' = f(y), y(0) = y0 using Rosenbrock method
//! Use semi-implicit Rosenbrock method using Jacobian evaluation. Equidistant steps of size T/N.
//! \tparam Func function type for r.h.s. f
//! \tparam DFunc function type for Jacobian df
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] f r.h.s. func f
//! \param[in] df Jacobian df of f
//! \param[in] y0 initial data y(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time
//! \return vector of y_k for each step k from 0 to N
template <class Func, class DFunc, class StateType>
std::vector<StateType> solveRosenbrock(const Func & f, const DFunc & df,
                                       const StateType & y0, unsigned int N, double T) {
    const double h = T/N;
    const double a = 1. / (std::sqrt(2) + 2.);

    // TODO: implement rosenbrock method
}

int main() {
    // Final time
    const double T = 10;
    // All mesh sizes
    const std::vector<int> N = {16, 32, 64, 128, 256, 512, 1024, 2048, 4096};
    // Reference mesh size
    const int N_ref = 16384;
    // Initial data
    Eigen::Vector2d y0;
    y0 << 1., 1.;

    // Function and his Jacobian
    // TODO: implement r.h.s. and Jacobian

    // TODO: compute reference solutions, solution, error and output approximate order of convergence
    }
}

Nummerical Methods for CSE/PS14/templates_ps14/rosenbrock_template.cpp


#include <boost/python.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <cmath>

#define MU_0 M_PI*4e-7

/* Functional code because making it into a class that is interfaceable with python is a massive
   sweffort.  It will instead be used and wrapped into a python class for ease of use.
 */

double ellipk(double _arg){
  return boost::math::ellint_1(_arg);
}

double ellipe(double _arg){
  return boost::math::ellint_2(_arg);
}

std::tuple<double, double> field_from_loop(double current, double radius, double R, double Z){
  double rho = sqrt(R*R + Z*Z);
  double alpha = sqrt(radius*radius + rho*rho - 2*radius*R);
  double beta = sqrt(radius*radius + rho*rho + 2*radius*R);
  double k = sqrt(1 - (alpha*alpha)/(beta*beta));

  double C = (MU_0*current)/M_PI;
  double a = 1.0/(2*(alpha*alpha)*beta);
  double b = Z/(2*(alpha*alpha)*beta*R);

  double Bz = C*a*((radius*radius - rho*rho)*ellipe(k) + (alpha*alpha)*ellipk(k));
  double Br = C*b*((radius*radius + rho*rho)*ellipe(k) - (alpha*alpha)*ellipk(k));

  if (std::isnan(Bz) == true || std::isinf(Bz) == true){
    Bz = 0;
  }
  if (std::isnan(Br) == true || std::isinf(Br) == true){
    Br = 0;
  }

  return std::make_tuple(Br, Bz);
}

std::tuple<double, double> make_current_layer(int nTurns, double separation, double startPos, double current, double radius, double R, double Z){

  double offset = startPos - separation;
  double newZ = 0;
  std::tuple<double, double> newBrBz (0, 0);
  double totalBr = 0;
  double totalBz = 0;

  for (int i = 0; i < nTurns; i++){
    offset += separation;
    newZ = Z - offset;
    newBrBz = field_from_loop(current, radius, R, newZ);
    totalBr += std::get<0>(newBrBz);
    totalBz += std::get<1>(newBrBz);
  }
  return std::make_tuple(totalBr, totalBz);

}

std::tuple<double, double> make_coil_from_layers(int nLayers, int nTurns, double layerSep, double loopSep, double startPos, double current, double minR, double R, double Z){
  double totalBr = 0;
  double totalBz = 0;

  std::tuple<double, double> newBrBz (0, 0);

  double radius = minR - layerSep;

  for (int i = 0; i < nLayers; i++){
    radius += layerSep;
    newBrBz = make_current_layer(nTurns, loopSep, startPos, current, radius, R, Z);
    totalBr += std::get<0>(newBrBz);
    totalBz += std::get<1>(newBrBz);
  }
  return std::make_tuple(totalBr, totalBz);
}

std::tuple<double, double> make_coil(double currentDensity, double centrePos, double length, double rInner, double rOuter, int nLayers, int nTurns, double R, double Z){
  double z1 = centrePos - 0.5*length;
  double z2 = centrePos + 0.5*length;
  double height = rOuter - rInner;

  double loopSep = length/double(nTurns);
  double layerSep = height/double(nLayers);

  double firstLoopZ = z1 + 0.5*loopSep;
  double firstLoopR = rInner + 0.5*layerSep;

  int totalLoops = nLayers*nTurns;
  double totalCurrent = length*height*currentDensity*1.0e6; //convert from A/mm^2 to A/m^2
  double currentPerLoop = totalCurrent/double(totalLoops);

  std::tuple<double, double> newBrBz = make_coil_from_layers(nLayers, nTurns, layerSep, loopSep, firstLoopZ, currentPerLoop, firstLoopR, R, Z);

  return newBrBz;
}

double current_to_density(double current, double rInner, double rOuter, double length, int nLayers, int nTurns){
  return (current*double(nLayers)*double(nTurns))/(length*(rOuter - rInner)*1e6); //in A/mm^2
}

std::tuple<double, double, double, double, double, double> convert_to_cartesian(double r, double phi, double z, double Br, double Bphi, double Bz){
  using namespace boost::numeric::ublas;
  double x = r*cos(phi);
  double y = r*sin(phi);

  //form vector of (Br, Bphi)
  vector<double> B(2);
  B(0) = Br; B(1) = Bphi;

  //form matrix that converts B_r/phi to B_x/y
  matrix<double> xy_rphi(2, 2);
  xy_rphi(0,0) = cos(phi); xy_rphi(0,1) = -1.0*sin(phi);
  xy_rphi(1,0) = sin(phi); xy_rphi(1,1) = cos(phi);

  vector<double> Bxy = prod(xy_rphi, B);

  return std::make_tuple(x, y, z, Bxy(0), Bxy(1), Bz);
}

std::tuple<double, double, double, double, double, double> convert_to_polar(double x, double y, double z, double Bx, double By, double Bz){
  using namespace boost::numeric::ublas;
  double r = sqrt(x*x + y*y);
  double phi = atan2(y, x);

  //form vector of (Bx, By)
  vector<double> B(2);
  B(0) = Bx; B(1) = By;

  //form matrix that converts B_x/y to B_r/phi
  matrix<double> rphi_xy(2, 2);
  rphi_xy(0, 0) = cos(phi); rphi_xy(0, 1) = sin(phi);
  rphi_xy(1, 0) = -1.0*sin(phi) ; rphi_xy(1, 1) = cos(phi);

  vector<double> Brphi = prod(rphi_xy, B);

  return std::make_tuple(r, phi, z, Brphi(0), Brphi(1), Bz);
}

std::tuple<double, double, double, double, double, double> rotate(double x, double y, double z, double Bx, double By, double Bz, double thetaX, double thetaY, double px, double py, bool transpose){
  using namespace boost::numeric::ublas;

  //set up vectors for x,y,z and Bxyz to manipulate on
  vector<double> X(3);
  X(0) = x; X(1) = y; X(2) = z;

  vector<double> B(3);
  B(0) = Bx; B(1) = By; B(2) = Bz;

  //set up offset vector
  vector<double> offset(3);
  offset(0) = px; offset(1) = py; offset(2) = 0;

  //set up rotation matrix
  matrix<double> R(3, 3);
  R(0, 0) = cos(thetaY); R(0, 1) = 0; R(0, 2) = sin(thetaY);
  R(1, 0) = sin(thetaX)*sin(thetaY); R(1, 1) = cos(thetaX); R(1, 2) = -1.0*cos(thetaY)*sin(thetaX);
  R(2, 0) = -1.0*sin(thetaY)*cos(thetaX); R(2, 1) = sin(thetaX); R(2, 2) = cos(thetaY)*cos(thetaX);

  if (transpose == false){
    vector<double> newX = prod(R, X) + offset;
    vector<double> newB = prod(R, B);
    return std::make_tuple(newX(0), newX(1), newX(2), newB(0), newB(1), newB(2));

  } else{
    vector<double> newX = prod(trans(R), X) - offset;
    vector<double> newB = prod(trans(R), B);
    return std::make_tuple(newX(0), newX(1), newX(2), newB(0), newB(1), newB(2));
  }
}

boost::python::tuple get_field_at_point(double current, double centrePos, double length, double rInner, double rOuter, int nLayers, int nTurns, double thetaX, double thetaY, double px, double py, double R, double PHI, double Z){
  double currentDensity = current_to_density(current, rInner, rOuter, length, nLayers, nTurns);

  //convert coords to cartesian
  auto tmp = convert_to_cartesian(R, PHI, Z, 0, 0, 0);

  //rotate with R
  tmp = rotate(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), 0, 0, 0, thetaX, thetaY, px, py, false);

  //convert back to polar
  tmp = convert_to_polar(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<3>(tmp), std::get<4>(tmp), std::get<5>(tmp));

  //now calculate the field
  std::tuple<double, double> newBrBz = make_coil(currentDensity, centrePos, length, rInner, rOuter, nLayers, nTurns, std::get<0>(tmp), std::get<2>(tmp));

  //convert to cartesian again INCLUDING B FIELD!!
  auto tmp_2 = convert_to_cartesian(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<0>(newBrBz), 0, std::get<1>(newBrBz));

  //Rotate with R_t
  tmp_2 = rotate(std::get<0>(tmp_2), std::get<1>(tmp_2), std::get<2>(tmp_2), std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2), thetaX, thetaY, px, py, true);

  //convert back to polars
  tmp_2 = convert_to_polar(std::get<0>(tmp_2), std::get<1>(tmp_2), std::get<2>(tmp_2), std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2));

  //Should check whether the coords are the same as R, PHI, Z

  return  boost::python::make_tuple(std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2));
}

//This func is like above but takes (x,y,z) as args and returns Bx, By, Bz for g4blgrid field
boost::python::tuple get_field_at_point_xyz(double current, double centrePos, double length, double rInner, double rOuter, int nLayers, int nTurns, double thetaX, double thetaY, double px, double py, double X, double Y, double Z){
  double currentDensity = current_to_density(current, rInner, rOuter, length, nLayers, nTurns);

  auto tmp = rotate(X, Y, Z, 0, 0, 0, thetaX, thetaY, px, py, false);

  tmp = convert_to_polar(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<3>(tmp), std::get<4>(tmp), std::get<5>(tmp));

  std::tuple<double, double> newBrBz =  make_coil(currentDensity, centrePos, length, rInner, rOuter, nLayers, nTurns, std::get<0>(tmp), std::get<2>(tmp));

  auto tmp_2 = convert_to_cartesian(std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<0>(newBrBz), 0, std::get<1>(newBrBz));

  tmp_2 = rotate(std::get<0>(tmp_2), std::get<1>(tmp_2), std::get<2>(tmp_2), std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2), thetaX, thetaY, px, py, true);

  return boost::python::make_tuple(std::get<3>(tmp_2), std::get<4>(tmp_2), std::get<5>(tmp_2));

}

BOOST_PYTHON_MODULE(makefield_cpp){
  using namespace boost::python;
  def ("get_field_at_point", get_field_at_point);
  def ("get_field_at_point_xyz", get_field_at_point_xyz);
}


#pragma once
#include <boost/multiprecision/cpp_int.hpp>
#include <tuple>
#include "utils.hpp"

namespace mp = boost::multiprecision;

namespace rsa {

    template<unsigned int N>
    class keys {

    public:

        using number = typename num_utils<N>::number;

        keys() {
            std::tie(_n, _e, _d, _phi) = generate_keys();
        }

        keys(const number& n , const number& e, const number& d, const number& phi) :
            _n(n), _e(e), _d(d), _phi(phi) {

        }

        static auto generate_keys() {
            const auto p = static_cast<number>(num_utils<N / 2>::generate_random_prime());
            const auto q = static_cast<number>(num_utils<N / 2>::generate_random_prime());
            const auto n = p * q;
            const auto phi = (p - 1) * (q - 1);
            const auto rand = num_utils<N>::get_int_random(2, phi - 1);
            auto e = rand();
            while (mp::gcd(phi, e) != 1)
                e = rand();
            const auto d = num_utils<N>::bezout_identity(e, phi).first % phi;
            return std::make_tuple(n, e, static_cast<number>(d > 0 ? d : phi + d), phi);
        }

        const auto& get_n() const {
            return _n;
        }

        const auto& get_e() const {
            return _e;
        }
        const auto& get_d() const {
            return _d;
        }
        const auto& get_phi() const {
            return _phi;
        }

    private:

        number _n, _e, _d, _phi;

    };

}// namespace rsa


#include <Eigen/Eigen>
#include <cfloat>
#include <iostream>
#include "GQ_Characterize.hpp"

GQ_Characterize::GQ_Characterize(double A, double B, double C,
                                 double D, double E, double F,
                                 double G, double H, double J,
                                 double K):
    A_(A),B_(B),C_(C),D_(D),E_(E),F_(F),G_(G),H_(H),J_(J),K_(K) {
    //determine canonical form of GQ and determine transformation
    make_canonical();
}

// The GQ Characterization method used here is based on work from
// Skip Thompson at Radford university
// https://www.radford.edu/~thompson/RP/quadrics.pdf
void GQ_Characterize::make_canonical()
{
  // create coefficient matrix
  Eigen::Matrix3f Aa;
  Aa << A_, D_/2, F_/2,
        D_/2, B_, E_/2,
        F_/2, E_/2, C_;

  // create hessian matrix
  Eigen::Matrix4f Ac;
  Ac << A_, D_/2, F_/2, G_/2,
        D_/2, B_, E_/2, H_/2,
        F_/2, E_/2, C_, J_/2,
        G_/2,  H_/2, J_/2, K_;

  // characterization values
  int rnkAa, rnkAc, delta, S, D;
  Eigen::FullPivLU<Eigen::Matrix3f> lu_decomp_Aa(Aa);
  Eigen::FullPivLU<Eigen::Matrix4f> lu_decomp_Ac(Ac);
  rnkAa = lu_decomp_Aa.rank();
  rnkAc = lu_decomp_Ac.rank();

  double determinant = Ac.determinant();
  if (fabs(determinant) < gq_tol)
    delta = 0;
  else
    delta = (determinant < 0) ? -1 : 1;

  Eigen::Vector3f eigenvals;
  Eigen::Matrix3f eigenvects;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> AaEigs;
  // completes Eiegen value computation, stored in AaEigs
  AaEigs.compute(Aa, Eigen::ComputeEigenvectors);
  eigenvals = AaEigs.eigenvalues();
  eigenvects = AaEigs.eigenvectors();
  Eigen::Vector3f signs;

  // determine signs of the eigenvalues
  for(unsigned int i = 0; i < 3; i++) {
    if (eigenvals[i] > -1 * gq_tol)
      signs[i] = 1;  //Sign counted as + if above -tolerance
    else
      signs[i] = -1; //Sign counted as - if less than -tolerance
  }

  // set S parameter based on agreement of eigenvalue signs
  S = (fabs(signs.sum()) == 3) ? 1 : -1;

  // may need to adjust delta for speical cases using the new scaling factor, K_
  // so we'll calculate that now
  Eigen::Vector3f b;
  b << -G_/2, -H_/2, -J_/2;

  // Use a Moore-Penrose pseudoinverse to ensure minimal norm least squares solution
  // this particular inverse ensures we get the "minimal" translation of the GQ
  // surface. A standard inverse may get the correct new basis set (rotation)
  // for the GQ, but with arbitrary scaling and in turn an arbitrarily scaled
  // translation vector
  Eigen::Matrix3f Aai = Aa.completeOrthogonalDecomposition().pseudoInverse();

  // Compute the translation from the inverse
  Eigen::Vector3f c = Aai * b;
  double dx = c[0], dy = c[1], dz = c[2];

  // Update the constant using the resulting translation
  K_ += (G_/2) * dx + (H_/2) * dy + (J_/2) * dz;

  // For the special case of the elliptic cylinder, delta will be needed.
  // Delta is set based on whether the sign of the equation constant, K, and the
  // signs of the eigenvalues are the same.
  if (rnkAa == 2 && rnkAc == 3 && S == 1) {
    delta = (K_ * signs[0]) ? -1 : 1;
  }

  D = (K_*signs[0]) ? -1:1;

  // characterize this GQ equation using the parameter values calculated above
  type = find_type(rnkAa, rnkAc, delta, S, D);
  // set the translation
  translation = Vector3d(dx,dy,dz);
  // set the rotaion matrix. LINE BELOW MAY BE UNNECESSARY but is saved in case
  // it is needed
  std::copy(eigenvects.data(),eigenvects.data()+9,rotation_mat);

  // update equation coefficients
  for(unsigned int i = 0; i < 3; i ++ ) if (fabs(eigenvals[i]) < gq_tol) eigenvals[i] = 0;
  A_ = eigenvals[0]; B_ = eigenvals[1]; C_ = eigenvals[2];
  D_ = 0; E_ = 0; F_ = 0;
  G_ = 0; H_ = 0; J_ = 0;
  // K is set above

  // simplify the GQ if possible
  reduce_type();
}

// this method reduces a complex GQ to a geometrically equivalent
// and more CAD-friendly form if appropriate
void GQ_Characterize::reduce_type() {

  if( ONE_SHEET_HYPERBOLOID == type ) {
    // if the K value is near-zero, reduce to Elliptic Cone
    if ( fabs(K_) < equivalence_tol ) {
      K_ = 0;
      type = ELLIPTIC_CONE;
      return;
    }
  }

  if ( TWO_SHEET_HYPERBOLOID == type ) {
    // if the K value is near-zero, reduce to Elliptic Cone
    if ( fabs(K_) < equivalence_tol ) {
      K_ = 0;
      type = ELLIPTIC_CONE;
      return;
    }
  }

  if ( ELLIPSOID == type ) {
    //if any of the 2nd order terms are near-zero, reduce to Elliptic Cylinder
    if ( fabs(A_) < equivalence_tol ) {
      A_ = 0;
      type = ELLIPTIC_CYL;
      return;
    }
    else if ( fabs(B_) < equivalence_tol ) {
      B_ = 0;
      type = ELLIPTIC_CYL;
      return;
    }
    else if ( fabs(C_) < equivalence_tol ) {
      C_ = 0;
      type = ELLIPTIC_CYL;
      return;
    }
  }

};

GQ_TYPE GQ_Characterize::find_type(int rt, int rf, int del, int s, int d) {

  GQ_TYPE t;
  if( 3 == rt && 4 == rf && -1 == del && 1 == s){
    t = ELLIPSOID;
  }
  else if( 3 == rt && 4 == rf && 1 == del && -1 == s){
    t = ONE_SHEET_HYPERBOLOID;
  }
  else if( 3 == rt && 4 == rf && -1 == del && -1 == s){
    t = TWO_SHEET_HYPERBOLOID;
  }
  else if( 3 == rt && 3 == rf && 0 == del && -1 == s){
    t = ELLIPTIC_CONE;
  }
  else if( 2 == rt && 4 == rf && -1 == del && 1 == s){
    t = ELLIPTIC_PARABOLOID;
  }
  else if( 2 == rt && 4 == rf && 1 == del && -1 == s){
    t = HYPERBOLIC_PARABOLOID;
  }
  else if( 2 == rt && 3 == rf && -1 == del && 1 == s){
    t = ELLIPTIC_CYL;
  }
  else if( 2 == rt && 3 == rf && 0 == del && -1 == s){
    t = HYPERBOLIC_CYL;
  }
  else if( 1 == rt && 3 == rf && 0 == del && 1 == s){
    t = PARABOLIC_CYL;
  }
  else{
    t = UNKNOWN;
  }

  //special case, replace delta with D
  if(2 == rt && 3 == rf && 1 == s && d != 0) {
    t = find_type(rt, rf, d, s, 0);
  }

  return t;
}


// Petter Strandmark 2012–2013.
#include <array>
#include <limits>

#include <Eigen/Dense>

#include <spii/solver.h>

namespace spii {

namespace
{
	// constexpr when supported.
	const double nan = std::numeric_limits<double>::quiet_NaN();
}

double
polynomial_interpolation(std::array<double, 2> x,
                         std::array<double, 2> f,
                         std::array<double, 2> df,
                         double xmin = nan, double xmax = nan)
{
	int minpos = 0;
	if (x[1] < x[0]) {
		minpos = 1;
	}
	int maxpos = 1 - minpos;

	if (xmin != xmin) {
		xmin = x[minpos];
	}
	if (xmax != xmax) {
		xmax = x[maxpos];
	}

	auto d1 = df[minpos] + df[maxpos] - 3*(f[minpos] - f[maxpos]) / (x[minpos] - x[maxpos]);
	auto inside_sqrt = d1*d1 - df[0]*df[1];
	if (inside_sqrt >= 0) {
		auto d2 = std::sqrt(inside_sqrt);
		auto t = x[maxpos] - (x[maxpos] - x[minpos])*((df[maxpos] + d2 - d1)/(df[maxpos] - df[minpos] + 2*d2));
		return std::min(std::max(t, xmin), xmax);
	}
	else {
		return (xmin + xmax) / 2;
	}
}

double perform_Wolfe_linesearch(const Solver& solver,
                                const Function& function,
                                const Eigen::VectorXd& x,
                                const double fval,
                                const Eigen::VectorXd& g,
                                const Eigen::VectorXd& p,
                                Eigen::VectorXd* scratch,
                                const double start_alpha)
{
	Eigen::VectorXd g_prev = g;
	auto f = function.evaluate(x);
	auto f_prev = f;
	double gtp = g.dot(p);
	auto gtp_prev = gtp;

	auto n = x.size();
	Eigen::VectorXd g_new(n);  // TODO: Somehow use scratch space.

	double alpha = start_alpha;
	double alpha_prev = 0;

	*scratch = x + alpha * p;
	double f_new = function.evaluate(*scratch, &g_new);
	double gtp_new  = g_new.dot(p);

	//
	auto c1 = solver.line_search_c;
	auto c2 = solver.line_search_c2;
	//

	std::array<double, 2> bracket;
	std::array<double, 2> bracket_fval;
	std::array<double, 2> bracket_gTpval;
	bool done = false;

	//
	// Bracketing phase.
	//
	int iterations = 0;
	const int max_iterations = 30;
	while (iterations <= max_iterations) {

		if (f_new > f + c1 * alpha * gtp || (iterations > 1 && f_new >= f_prev)) {
			// Double braces for GCC 4.7 compatibility. Remove later.
			bracket        = {{alpha_prev, alpha}};
			bracket_fval   = {{f_prev, f_new}};
			bracket_gTpval = {{ g_prev.dot(p), g_new.dot(p)}};
			break;
		}
		else if (std::abs(gtp_new) <= -c2 * gtp) {
			// We are done.
			return alpha;
		}
		else if (gtp_new >= 0) {
			// Double braces for GCC 4.7 compatibility. Remove later.
			bracket        = {{alpha_prev, alpha}};
			bracket_fval   = {{f_prev, f_new}};
			bracket_gTpval = {{g_prev.dot(p), g_new.dot(p)}};
			break;
		}

		double temp = alpha_prev;
		alpha_prev = alpha;
		double minStep = alpha + 0.01*(alpha - temp);
		double maxStep = 10 * alpha;

		if (solver.wolfe_interpolation_strategy == Solver::BISECTION) {
			alpha = maxStep;
		}
		else {
			// Double braces for GCC 4.7 compatibility. Remove later.
			alpha = polynomial_interpolation({{temp, alpha}},
			                                 {{f_prev, f_new}},
			                                 {{gtp_prev, gtp_new}},
			                                 minStep, maxStep);
		}

		f_prev = f_new;
		g_prev = g_new;
		gtp_prev = gtp_new;

		*scratch = x + alpha * p;
		f_new = function.evaluate(*scratch, &g_new);
		gtp_new  = g_new.dot(p);

		iterations++;
	}

	//
	// Zoom phase.
	//
	bool insufficient_progress = false;
	while (!done && iterations <= max_iterations) {

		// Compute new trial value.
		if (solver.wolfe_interpolation_strategy == Solver::BISECTION) {
			alpha = (bracket[0] + bracket[1]) / 2.0;
		}
		else {
			alpha = polynomial_interpolation(bracket,
			                                 bracket_fval,
			                                 bracket_gTpval);
		}

		auto max_bracket = std::max({bracket[0], bracket[1]});
		auto min_bracket = std::min({bracket[0], bracket[1]});

		if (std::min({max_bracket-alpha, alpha-min_bracket}) / (max_bracket - min_bracket) < 0.1) {
			if (insufficient_progress || alpha >= max_bracket || alpha <= min_bracket) {
				if (std::abs(alpha - max_bracket) < std::abs(alpha - min_bracket)) {
					alpha = max_bracket - 0.1*(max_bracket - min_bracket);
				}
				else {
					alpha = min_bracket + 0.1*(max_bracket - min_bracket);
				}
				insufficient_progress = false;
			}
			else {
				insufficient_progress = true;
			}
		}

		*scratch = x + alpha * p;
		f_new = function.evaluate(*scratch, &g_new);
		gtp_new  = g_new.dot(p);

		int lo_pos, hi_pos;
		double f_low;
		if (bracket_fval[0] < bracket_fval[1]) {
			f_low = bracket_fval[0];
			lo_pos = 0;
			hi_pos = 1;
		}
		else {
			f_low = bracket_fval[1];
			lo_pos = 1;
			hi_pos = 0;
		}

		bool armijo = f_new < f + c1 * alpha * gtp;

		if (!armijo || f_new >= f_low) {
			// Armijo condition not satisfied or not lower than lowest
			// point
			bracket[hi_pos]        = alpha;
			bracket_fval[hi_pos]   = f_new;
			bracket_gTpval[hi_pos] = g_new.dot(p);
		}
		else {
			if (std::abs(gtp_new) <= - c2*gtp) {
				// Wolfe conditions satisfied
				done = true;
			}
			else if (gtp_new * (bracket[hi_pos] - bracket[lo_pos]) >= 0) {
				// Old HI becomes new LO
				bracket[hi_pos]        = bracket[lo_pos];
				bracket_fval[hi_pos]   = bracket_fval[lo_pos];
				bracket_gTpval[hi_pos] = bracket_gTpval[lo_pos];
			}

			// New point becomes new LO
			bracket[lo_pos]        = alpha;
			bracket_fval[lo_pos]   = f_new;
			bracket_gTpval[lo_pos] = g_new.dot(p);
		}

		iterations++;
	}

	if (!done) {
		if (solver.log_function) {
			solver.log_function("Wolfe line search maximum iterations exceeded.");
		}
		return 0.0;
	}

	if (bracket_fval[0] < bracket_fval[1]) {
		alpha = bracket[0];
	}
	else {
		alpha = bracket[1];
	}

	return alpha;
}

double perform_Armijo_linesearch(const Solver& solver,
                                 const Function& function,
                                 const Eigen::VectorXd& x,
                                 const double fval,
                                 const Eigen::VectorXd& g,
                                 const Eigen::VectorXd& p,
                                 Eigen::VectorXd* scratch,
                                 const double start_alpha)
{
	//
	// Perform back-tracking line search.
	//

	// Starting value for alpha during line search. Newton and
	// quasi-Newton methods should choose 1.0.
	double alpha = start_alpha;
	double rho = solver.line_search_rho;
	double c = solver.line_search_c;
	double gTp = g.dot(p);
	if (gTp != gTp) {
		if (solver.log_function) {
			solver.log_function("Backtracking encountered NaN, returning zero step.");
		}
		return 0.0;
	}

	int backtracking_attempts = 0;
	while (true) {
		*scratch = x + alpha * p;
		double lhs = function.evaluate(*scratch);
		double rhs = fval + c * alpha * gTp;
		if (lhs <= rhs) {
			break;
		}
		alpha *= rho;

		backtracking_attempts++;
		if (backtracking_attempts > 1000) {
			if (solver.log_function) {
				solver.log_function("Backtracking failed, returning zero step.");
			}
			return 0.0;
		}
	}

	return alpha;
}

double Solver::perform_linesearch(const Function& function,
                                  const Eigen::VectorXd& x,
                                  const double fval,
                                  const Eigen::VectorXd& g,
                                  const Eigen::VectorXd& p,
                                  Eigen::VectorXd* scratch,
                                  const double start_alpha) const
{
	if (this->line_search_type == ARMIJO) {
		return perform_Armijo_linesearch(*this, function, x, fval, g, p, scratch, start_alpha);
	}
	else {
		return perform_Wolfe_linesearch(*this, function, x, fval, g, p, scratch, start_alpha);
	}
}

}  // namespace spii


#include "Mesh3D.hpp"

#include <Eigen/Geometry>

float noob::mesh_3d::get_volume()
{
	if (!volume_calculated)
	{
		// Proudly gleaned from U of R website!
		// http://mathcentral.uregina.ca/QQ/database/QQ.09.09/h/ozen1.html
		// The volume of the tetrahedron with vertices (0 ,0 ,0), (a1 ,a2 ,a3), (b1, b2, b3) and (c1, c2, c3) is [a1b2c3 + a2b3c1 + a3b1c2 - a1b3c2 - a2b1c3 - a3b2c1] / 6.
		double accum = 0.0;
		for (uint32_t i = 0; i < indices.size(); i += 3)
		{
			const noob::vec3f first = vertices[i].position;
			const noob::vec3f second = vertices[i+1].position;
			const noob::vec3f third = vertices[i+2].position;

			accum += ((first[0] * second[1] * third[2]) + (first[1] * second[2] * third[0]) + (first[2] * second[0] * third.v[1]) - (first[0] * second.v[2] * third.v[1]) - (first.v[1] * second[0] * third.v[2]) - (first[2] * second[1] * third.v[0])) / 6.0;

		}

		volume_calculated = true;
		volume = static_cast<float>(accum);
	}

	return volume;
}

void noob::mesh_3d::calculate_dims() noexcept(true)
{
	if (vertices.size() > 0)
	{
		bbox.min = bbox.max = vertices[0].position;
		for (noob::mesh_3d::vert v : vertices)
		{
			bbox.min[0] = std::min(bbox.min[0], v.position[0]);
			bbox.min[1] = std::min(bbox.min[1], v.position[1]);
			bbox.min[2] = std::min(bbox.min[2], v.position[2]);
			bbox.max[0] = std::max(bbox.max[0], v.position[0]);
			bbox.max[1] = std::max(bbox.max[1], v.position[1]);
			bbox.max[2] = std::max(bbox.max[2], v.position[2]);
		}
	}
}

/*
   void noob::mesh_3d::to_origin()
   {
   noob::vec3f dims = bbox.get_dims();
   for (size_t i = 0; i < vertices.size(); ++i)
   {
   vertices[i] = (vertices[i] + dims);
   }
   }

   std::string noob::mesh_3d::save() const
   {
   fmt::MemoryWriter w;
   w << "OFF" << "\n" << vertices.size() << " " << indices.size() / 3 << " " << 0 <<  "\n";
   for (auto v : vertices)
   {
   w << v.v[0] << " " << v.v[1] << " " << v.v[2] <<  "\n";
   }
   for (size_t i = 0; i < indices.size(); i = i + 3)
   {
   w << 3 << " " << indices[i] << " " << indices[i+1] << " " << indices[i+2] << "\n";
   }

   return w.str();
   }

   void noob::mesh_3d::save(const std::string& filename) const
   {

   }
   */


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

#include <votca/tools/cubicspline.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <votca/tools/linalg.h>
#include <iostream>
#include <cmath>

namespace votca { namespace tools {

using namespace std;

void CubicSpline::Interpolate(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in CubicSpline::Interpolate : sizes of vectors x and y do not match");

    if(x.size()<3)
        throw std::invalid_argument("error in CubicSpline::Interpolate : vectors x and y have to contain at least 3 points");

    const int N = x.size();

    // adjust the grid
    _r.resize(N);
    _f.resize(N);
    _f2.resize(N);

    // create vector proxies to individually access f and f''

    // copy the grid points into f
    _r = x;
    _f = y;
    _f2 = ub::zero_vector<double>(N);

    // not calculate the f''
    ub::matrix<double> A(N, N);
    A = ub::zero_matrix<double>(N,N);

    for(int i=0; i<N - 2; ++i) {
            _f2(i+1) = -( A_prime_l(i)*_f(i)
            + (B_prime_l(i) - A_prime_r(i)) * _f(i+1)
            -B_prime_r(i) * _f(i+2));

            A(i+1, i) = C_prime_l(i);
            A(i+1, i+1) = D_prime_l(i) - C_prime_r(i);
            A(i+1, i+2) = -D_prime_r(i);
    }

    switch(_boundaries) {
        case splineNormal:
            A(0, 0) = 1;
            A(N - 1, N-1) = 1;
            break;
        case splinePeriodic:
            A(0,0) = 1; A(0,N-1) = -1;
            A(N-1,0) = 1; A(N-1,N-1) = -1;
            break;
        case splineDerivativeZero:
	    throw std::runtime_error("erro in CubicSpline::Interpolate: case splineDerivativeZero not implemented yet");
	    break;
    }

    votca::tools::linalg_qrsolve(_f2, A, _f2);
}

void CubicSpline::Fit(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in CubicSpline::Fit : sizes of vectors x and y do not match");

    const int N = x.size();
    const int ngrid = _r.size();

    // construct the equation
    // A*u = b
    // where u = { {f[i]}, {f''[i]} }
    // and b[i] = y[i] for 0<=i<N
    // and b[i]=0 for i>=N (for smoothing condition)
    // A[i,j] contains the data fitting + the spline smoothing conditions

    ub::matrix<double> A(N, 2*ngrid);
    ub::vector<double> b(N);
    ub::matrix<double> B_constr(ngrid, 2*ngrid);  // Matrix with smoothing conditions

    A = ub::zero_matrix<double>(N, 2*ngrid);
    b  = ub::zero_vector<double>(N);
    B_constr = ub::zero_matrix<double>(ngrid, 2*ngrid);

    // Construct smoothing matrix
    AddBCToFitMatrix(B_constr, 0);

    // construct the matrix to fit the points and the vector b
    AddToFitMatrix(A, x, 0);
    b = -y; // why is it -y?

    // now do a constrained qr solve
    ub::vector<double> sol(2*ngrid);
    votca::tools::linalg_constrained_qrsolve(sol, A, b, B_constr);

    // check vector "sol" for nan's
    for(int i=0; i<2*ngrid; i++) {
        if( (isinf(sol(i))) || (isnan(sol(i))) ) {
            throw std::runtime_error("error in CubicSpline::Fit : value nan occurred due to wrong fitgrid boundaries");
        }
    }

    _f = ub::vector_range<ub::vector<double> >(sol, ub::range (0, ngrid));
    _f2 = ub::vector_range<ub::vector<double> >(sol, ub::range (ngrid, 2*ngrid));
}

}}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2021, Oracle and/or its affiliates
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

//[azimuth_strategy
//` Shows how to calculate azimuth in geographic coordinate system

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>

int main()
{
    namespace bg = boost::geometry;
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point_type;

    point_type p1(0, 0);
    point_type p2(1, 1);

    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::strategies::azimuth::geographic<> strategy(spheroid);

    auto azimuth = boost::geometry::azimuth(p1, p2, strategy);

    std::cout << "azimuth: " << azimuth << std::endl;

    return 0;
}

//]

//[azimuth_strategy_output
/*`
Output:
[pre
azimuth: 0.788674
]
*/
//]


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Andres Hernandez

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file levyflightdistribution.hpp
    \brief Levy Flight, aka Pareto Type I, distribution as needed by Boost Random
*/

#ifndef quantlib_levy_flight_distribution_hpp
#define quantlib_levy_flight_distribution_hpp

#include <ql/types.hpp>
#include <ql/errors.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>
#include <iosfwd>

namespace QuantLib {

    //! Levy Flight distribution as needed by Boost Random
    /*! The levy flight distribution is a random distribution with
        the following form:
        \f[
        p(x) = \frac{\alpha x_m^{\alpha}}{x^{\alpha+1}}
        \f]
        with support over \f$ x \in [x_m, \infty) \f$
        and the parameter \f$ \alpha > 0 \f$.

        Levy Flight is normally defined as \f$ x_m = 1 \f$ and \f$ 0 <
        \alpha < 2 \f$, which is where \f$ p(x) \f$ has an infinite
        variance. However, the more general version, known as Pareto
        Type I, is well defined for \f$ \alpha > 2 \f$, so the current
        implementation does not restrict \f$ \alpha \f$ to be smaller
        than 2.
    */
    class LevyFlightDistribution
    {
      public:
        typedef Real input_type;
        typedef Real result_type;

        class param_type
        {
          public:

            typedef LevyFlightDistribution distribution_type;

            /*!    Constructs parameters with a given xm and alpha
                Requires: alpha > 0
            */
            param_type(Real xm = 1.0, Real alpha = 1.0)
              : xm_(xm), alpha_(alpha) { QL_REQUIRE(alpha_ > 0.0, "alpha must be larger than 0"); }

            //! Returns the xm parameter of the distribution
            Real xm() const { return xm_; }

            //! Returns the alpha parameter of the distribution
            Real alpha() const { return alpha_; }

            //! Writes the parameters to a @c std::ostream
            BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
            {
                os << parm.xm_ << " " << parm.alpha_;
                return os;
            }

            //! Reads the parameters from a @c std::istream
            BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
            {
                is >> parm.xm_ >> std::ws >> parm.alpha_;
                return is;
            }

            //! Returns true if the two sets of parameters are equal
            BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
            { return lhs.xm_ == rhs.xm_ && lhs.alpha_ == rhs.alpha_; }

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

        private:
            Real xm_;
            Real alpha_;
        };

        //! \name Constructors
        //@{
        /*! Constructs a LevyFlightDistribution with a given xm and alpha
            Requires: alpha > 0
        */
        explicit LevyFlightDistribution(Real xm = 1.0, Real alpha = 1.0)
          : xm_(xm), alpha_(alpha) { QL_REQUIRE(alpha_ > 0.0, "alpha must be larger than 0"); }

        //!Constructs a LevyFlightDistribution from its parameters
        explicit LevyFlightDistribution(const param_type& parm)
          : xm_(parm.xm()), alpha_(parm.alpha()) {}

        // compiler-generated copy ctor and assignment operator are fine
        //@}

        //! \name Inspectors
        //@{
        //! Returns the xm parameter of the distribution
        Real xm() const { return xm_; }

        //! Returns the alpha parameter of the distribution
        Real alpha() const { return alpha_; }

        //! Returns the smallest value that the distribution can produce
        Real min BOOST_PREVENT_MACRO_SUBSTITUTION () const
        { return xm_; }
        //! Returns the largest value that the distribution can produce
        Real max BOOST_PREVENT_MACRO_SUBSTITUTION () const
        { return QL_MAX_REAL; }

        //! Returns the parameters of the distribution
        param_type param() const { return {xm_, alpha_}; }
        //@}

        //! Sets the parameters of the distribution
        void param(const param_type& parm) {
            xm_ = parm.xm();
            alpha_ = parm.alpha();
        }

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

        //! Returns the value of the pdf for x
        Real operator()(Real x) const{
            using std::pow;
            if(x < xm_) return 0.0;
            return alpha_*pow(xm_/x, alpha_)/x;
        }

        /*!    Returns a random variate distributed according to the
            levy flight distribution.
        */
        template<class Engine>
        result_type operator()(Engine& eng) const{
            using std::pow;
            return xm_*pow(boost::random::uniform_01<Real>()(eng), -1.0/alpha_);
        }

        /*!    Returns a random variate distributed according to the
            levy flight with parameters specified by parm
        */
        template<class Engine>
        result_type operator()(Engine& eng, const param_type& parm) const{
            return LevyFlightDistribution (parm)(eng);
        }

        //! Writes the distribution to a std::ostream
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, LevyFlightDistribution, ed)
        {
            os << ed.xm_ << " " << ed.alpha_;
            return os;
        }

        //! Reads the distribution from a std::istream
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, LevyFlightDistribution, ed)
        {
            is >> ed.xm_ >> std::ws >> ed.alpha_;
            return is;
        }

        /*! Returns true iff the two distributions will produce identical
            sequences of values given equal generators.
        */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(LevyFlightDistribution, lhs, rhs)
        { return lhs.xm_ == rhs.xm_ && lhs.alpha_ == rhs.alpha_; }

        /*!    Returns true iff the two distributions will produce different
            sequences of values given equal generators.
        */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(LevyFlightDistribution)

    private:
        result_type xm_;
        result_type alpha_;
    };

}

#endif


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <manifold/SO3.h>
#include <manifold/gradientDescentSE3.h>

class GDSE3gmm : public GDSE3<double> {
 public:
  GDSE3gmm(const Eigen::Vector3d& muA, const
      Eigen::Matrix3d& covA, const Eigen::Vector3d& muB, const
      Eigen::Matrix3d& covB)
    : piA_(1.), piB_(1.), muA_(muA), muB_(muB), covA_(covA), covB_(covB)
  {
    std::cout << "-A-"
      << muA.transpose() << std::endl
      << covA << std::endl;
    std::cout << "-B-"
      << muB.transpose() << std::endl
      << covB << std::endl;
  };

  virtual void ComputeJacobian(const SE3d& theta, Eigen::Matrix<double,6,1>* J, double* f) {
    SE3d T = theta;
    Eigen::Matrix3d R = T.matrix().topLeftCorner(3,3);
    Eigen::Vector3d t = T.matrix().topRightCorner(3,1);
    // TODO: maybe this has to be negated?
    Eigen::Vector3d m = (R*muB_ - muA_);
    Eigen::Matrix3d SB = R*covB_*R.transpose();
    Eigen::Matrix3d S = covA_+SB;
    double logCA = -0.5*log(2.*M_PI)*3-0.5*covA_.determinant();
    double logCB = -0.5*log(2.*M_PI)*3-0.5*SB.determinant();
    double logD = log(piA_) + log(piB_) + logCA +logCB;
    double z = -0.5*(t-m).dot(S.ldlt().solve(t-m));
//    std::cout << "logCA=" << logCA << " logCB=" << logCB << std::endl;
//    std::cout << "logD=" << logD << " z=" << z
//      << " exp(logD+z)=" << exp(logD + z) << std::endl;
    if (J) {
      J->fill(exp(logD + z));
      J->topRows(3) = J->topRows(3).array()*(-S.ldlt().solve(t-m)).array();
      for (uint32_t j=0; j<3; ++j) {
        Eigen::Matrix3d G = SO3d::G(j);
        Eigen::Matrix3d C = G*SB+SB*G.transpose();
        Eigen::Matrix3d SinvC = S.ldlt().solve(C);
        Eigen::Matrix3d SinvG = S.ldlt().solve(G);
        Eigen::Matrix3d Sinv = S.inverse();

//        std::cout << "G = "<<G<<std::endl
//         << "C = "         <<C<<std::endl
//         << "SinvC ="<<  SinvC<<std::endl
//         << "SinvG ="<<  SinvG<<std::endl
//         << "Sinv = "<<  Sinv<<std::endl;
//         std::cout << G.transpose()*Sinv << std::endl
//           << SinvC*Sinv << std::endl
//           << SinvG << std::endl;
//            << (G.transpose()*Sinv-SinvC*Sinv+SinvG) << std::endl;
//           << C << std::endl;

        std::cout << "@G" << j
           << "\t" << SinvC.trace()
           << "\t" << - t.dot(SinvC*S.ldlt().solve(t))
           << "\t" << -2.*t.dot(SinvC*S.ldlt().solve(muA_))
           << "\t" << -2.*t.dot((-SinvC*S.ldlt().solve(R)+SinvG*R)*muB_)
           << "\t" << (R*muB_).dot((G.transpose()*Sinv-SinvC*Sinv+SinvG)*R*muB_)
           << "\t" << +2.*(R*muB_).dot((SinvG*(Eigen::Matrix3d::Identity()-SB*Sinv))*R*muB_)
           << std::endl;

        (*J)(3+j) *= -0.5*(
            SinvC.trace()
            - t.dot(SinvC*S.ldlt().solve(t))
            -2.*t.dot(SinvC*S.ldlt().solve(muA_))
            -2.*t.dot((-SinvC*S.ldlt().solve(R)+SinvG*R)*muB_)
            +(R*muB_).dot((G.transpose()*Sinv-SinvC*Sinv+SinvG)*R*muB_));
      }
      (*J) *= -1.;
//      J->bottomRows(3).fill(0.);
    }
    if (f) {
      *f = -exp(logD + z);
    }
  };
 protected:
  SO3d Rmu_;
  double piA_;
  double piB_;
  Eigen::Vector3d muA_;
  Eigen::Vector3d muB_;
  Eigen::Matrix3d covA_;
  Eigen::Matrix3d covB_;
};

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

  double theta = 15.*M_PI/180.;
  Eigen::Matrix3d R;
  R << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
//  R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Ones();
  Eigen::Matrix3d covA =   Eigen::Vector3d(1.,.1,1.).asDiagonal();
  Eigen::Matrix3d covB = R*covA*R.transpose();
  Eigen::Vector3d muA = Eigen::Vector3d::Zero();
  Eigen::Vector3d muB = R*muA+t;

  SE3d T;

  GDSE3gmm gd(muA, covA, muB, covB);
  gd.Compute(T, 1e-6, 200);
//  gd.Compute(T, 0, 200);
  T = gd.GetMinimum();
  Eigen::Vector3d tEst = T.matrix().topRightCorner(3,1);
  Eigen::Matrix3d REst = T.matrix().topLeftCorner(3,3);
  std::cout << T << std::endl;
  std::cout << R << std::endl;
  std::cout << REst.transpose() << std::endl;
  std::cout << (-REst.transpose()*tEst).transpose() << std::endl;
  std::cout << t.transpose() << std::endl;

}


#include "VPD.h"
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#include <lsd.h>
#include <iostream>
#include <cmath>
#include <boost/numeric/ublas/lu.hpp>

#include "VPCluster.h"
#include "VPSample.h"

std::vector< std::pair<float, float> > 	extractVP 		(image_double &image, const cv::Mat &img);
cv::Mat 								makeFeatureMap	(std::vector< std::pair<float, float> > &vp, const cv::Mat &img, double *reliability = NULL);

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

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

	// more intuative this way
	nDegree++;

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

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

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

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

	// lu decomposition
	permutation_matrix<T> pert(oXtXMatrix.size1());
	const std::size_t singular = lu_factorize(oXtXMatrix, pert);
	// must be singular
	if( singular != 0 ) {
		// then the regression cannot be done... return an empty vector
		return std::vector<T>();
	}

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

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

template<typename T>
std::vector<T> polyval( const std::vector<T>& oCoeff,  const std::vector<T>& oX ) {
	size_t nCount =  oX.size();
	size_t nDegree = oCoeff.size();
	std::vector<T>	oY( nCount );

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

	return oY;
}

struct Segment {
	float x;
	float y;
	float xend;
	float yend;
	float angle;
};

float  horizonLine(cv::Mat &image) {
	if(image.empty()) return 0;

	std::vector<float> x, y;

	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

	cv::Mat edges;
	cv::Canny(gray, edges, 110, 220);

	std::vector<cv::Vec4i> lines;
    cv::HoughLinesP( edges, lines, 1, CV_PI/2, 0, 30, 1 );
    float meanY = 0; int nbLines = 0;
    float meanY2 = 0;
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	if(lines[i][0] == lines[i][2]) continue;

		if(lines[i][1] > (.7 * image.rows)) continue;
		if(lines[i][1] < (.3 * image.rows)) continue;

        cv::line( image, cv::Point(lines[i][0], lines[i][1]),
            cv::Point(lines[i][2], lines[i][3]), cv::Scalar(0,0,255), 3, 8 );

        x.push_back(lines[i][0]);
		x.push_back(lines[i][2]);
		y.push_back(lines[i][1]);
		y.push_back(lines[i][3]);

		meanY += lines[i][1]; ++nbLines;
		meanY += lines[i][3]; ++nbLines;

		meanY2 += lines[i][1] * lines[i][1];
		meanY2 += lines[i][3] * lines[i][3];

    }
    meanY /= nbLines;
    meanY2 /= nbLines;

    float stdev = std::sqrt(meanY2 - meanY*meanY);
    float meanValue = meanY;

    meanY = 0; nbLines = 0;
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	if(lines[i][0] == lines[i][2]) continue;

		if(lines[i][1] > (.7 * image.rows)) continue;
		if(lines[i][1] < (.3 * image.rows)) continue;

        if(std::abs(lines[i][1] - meanValue) > 1.3 * stdev) continue;

		meanY += lines[i][1]; ++nbLines;
		meanY += lines[i][3]; ++nbLines;

    }
    meanY /= nbLines;

    if(nbLines == 0) {
    	meanY = image.rows/2;
    } else {
    	std::vector<float> model = polyfit<float>(x, y, 1);

	    if(std::abs(model[0]- image.rows/2) < std::abs(meanY - image.rows/2))
	    	meanY = model[0];
    }

	cv::line(image, cv::Point_<int>(0, meanY), cv::Point_<int>(image.cols, meanY), cv::Scalar( 0, 255, 0 ), 2);

return meanY;

}

std::vector< std::pair<float, float> >  extractVP(const cv::Mat &image) {

	// ------------------------------------------------------------------------------------
	// convert image input

	if(image.empty()) return std::vector< std::pair<float, float> >();

	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
	cv::Mat grayF;
	gray.convertTo(grayF, CV_32FC1);

	image_double image_d = new_image_double(gray.cols, gray.rows);
	for(int i = 0 ; i < gray.cols*gray.rows ; ++i) {
		image_d->data[i] = reinterpret_cast<float*>(grayF.data)[i];
	}

	return extractVP(image_d, image);

}

std::vector< std::pair<float, float> > extractVP(image_double &image_d, const cv::Mat &) {

	// cv::Mat colorImg = image.clone();

	// ------------------------------------------------------------------------------------
	// Run LSD line segment detector

	ntuple_list ntuple = LineSegmentDetection( image_d, 0.7,
                                  0.6, 2.6,
                                  22.5, 0.0, 0.6,
                                  1024, 1.0,
                                  NULL );

	// ntuple_list ntuple = LineSegmentDetection( image_d, 0.7,
    //                               0.6, 2.6,
    //                               22.5, 0.0, 0.6,
    //                               1024, 1.0,
    //                               NULL );

	std::vector< std::vector<float> *> pts;

	std::vector<float> stroke_length(ntuple->size);
	for(size_t i = 0 ; i < ntuple->size ; ++i) {
		stroke_length[i] = static_cast<float>( (ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ])*(ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ]) + (ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ])*(ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ]));
	}

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

	if(stroke_length.empty()) {
		free_image_double(image_d);
		free_ntuple_list(ntuple);
		return std::vector< std::pair<float, float> >();
	}

	// std::cout << stroke_length[static_cast<int>(stroke_length.size()*.7)] << std::endl;

	 // std::cout << ntuple->size << "line segments found:\n";
	for(size_t i = 0 ; i < ntuple->size ; ++i) {
		float len = static_cast<float>( (ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ])*(ntuple->values[ i * ntuple->dim + 0 ] - ntuple->values[ i * ntuple->dim + 2 ]) + (ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ])*(ntuple->values[ i * ntuple->dim + 1 ] - ntuple->values[ i * ntuple->dim + 3 ]) );
		if(len < stroke_length[static_cast<int>(stroke_length.size()*.7)]) continue;
		if(len < 827.748f) continue;

		std::vector<float>* p = new std::vector<float>(4);
		pts.push_back(p);
		for(size_t j = 0 ; j < ntuple->dim ; ++j) {
			if(j < 4) (*p)[j] = static_cast<float>(ntuple->values[ i * ntuple->dim + j ]);
			// std::cout << ntuple->values[ i * ntuple->dim + j ] << " ";
		}
		 // std::cout << "\n";
	}

	free_image_double(image_d);
	free_ntuple_list(ntuple);

	// for(size_t i = 0 ; i < pts.size() ; ++i) {
	// 	cv::line(colorImg, cv::Point((*pts[i])[0], (*pts[i])[1]),  cv::Point((*pts[i])[2], (*pts[i])[3]), cv::Scalar(255,0,0),2);
	// }

	// ------------------------------------------------------------------------------------
	// JLinkage for cluster detection

	std::vector<unsigned int> Lables;
	std::vector<unsigned int> LableCount;

	if(pts.size() < 20) {
		for(size_t i = 0 ; i < pts.size() ; ++i) {
			delete pts[i];
		}

		return std::vector< std::pair<float, float> >();
	}

	std::vector<std::vector<float> *> *mModels =
		VPSample::run(&pts, 5000, 2, 0, 3); // VPSample::run(&pts, 5000, 2, 0, 3);
	int classNum = VPCluster::run(Lables, LableCount, &pts, mModels, 2, 2);
	// std::cout<<"vpdetection found "<<classNum<<" classes!"<<std::endl;

	//2.1. release other resource
	for(unsigned int i=0; i < mModels->size(); ++i)
		delete (*mModels)[i];
	delete mModels;

	// ------------------------------------------------------------------------------------
	// Compute the vanishing point positions

	std::vector< std::vector<float> > lengths(Lables.size());
	std::vector<float> dydx(pts.size());
	std::vector<float> b(pts.size());
	std::vector<bool>  disabled(pts.size(), false);

	for(size_t i = 0 ; i < pts.size() ; ++i) {
		lengths[Lables[i]].push_back(std::sqrt(((*pts[i])[1]-(*pts[i])[3])*((*pts[i])[1]-(*pts[i])[3]) +  ((*pts[i])[0]-(*pts[i])[2])*((*pts[i])[0]-(*pts[i])[2])));
		if(std::abs(((*pts[i])[0] - (*pts[i])[2])) < 0.00001) {
			disabled[i] = true;
			continue;
		}

		dydx[i] = ((*pts[i])[1] - (*pts[i])[3]) / ((*pts[i])[0] - (*pts[i])[2]);
		b[i] =  (*pts[i])[1] - dydx[i] * (*pts[i])[0];

		if(atan(std::abs(dydx[i])) < (20*3.141592653/180)) {
			disabled[i] = true;
		}

	}

	for(int i = 0 ; i < classNum ; ++i) {
		std::sort(lengths[i].begin(), lengths[i].end());
	}

	std::vector<float> numPoints(classNum, 0);
	std::vector<float> vp_x(classNum, 0);
	std::vector<float> vp_y(classNum, 0);

	for(size_t ii = 0 ; ii < pts.size() ; ++ii) {
		for(size_t jj = ii+1 ; jj < pts.size() ; ++jj) {

			float l1 = std::sqrt(((*pts[ii])[1]-(*pts[ii])[3])*((*pts[ii])[1]-(*pts[ii])[3]) +  ((*pts[ii])[0]-(*pts[ii])[2])*((*pts[ii])[0]-(*pts[ii])[2]));
			float l2 = std::sqrt(((*pts[jj])[1]-(*pts[jj])[3])*((*pts[jj])[1]-(*pts[jj])[3]) +  ((*pts[jj])[0]-(*pts[jj])[2])*((*pts[jj])[0]-(*pts[jj])[2]));

			if(Lables[ii] != Lables[jj]) continue;
			if(disabled[ii] || disabled[jj]) continue;
			if(lengths[Lables[ii]].size() == 0) continue;
			if(lengths[Lables[jj]].size() == 0) continue;

			if(l1 < lengths[Lables[ii]][static_cast<int>(lengths[Lables[ii]].size()*.7)]) continue;
			if(l2 < lengths[Lables[jj]][static_cast<int>(lengths[Lables[jj]].size()*.7)]) continue;

			if(std::abs((dydx[jj] - dydx[ii])) < 0.000001) continue;

			float x = (b[ii] - b[jj]) / (dydx[jj] - dydx[ii]);
			float y = dydx[ii] * x + b[ii];

			// cv::line(colorImg, cv::Point((*pts[ii])[0], (*pts[ii])[1]),  cv::Point((*pts[ii])[2], (*pts[ii])[3]), cv::Scalar(0,255,0),2);
			// cv::line(colorImg, cv::Point((*pts[jj])[0], (*pts[jj])[1]),  cv::Point((*pts[jj])[2], (*pts[jj])[3]), cv::Scalar(0,0,255),2);

			vp_x[Lables[ii]] += x;
			vp_y[Lables[ii]] += y;
			++numPoints[Lables[ii]];
		}
	}

	std::vector< std::pair<float, float> > vp;
	int considered_idx = 0;
	for(int i = 0 ; i < classNum ; ++i) {
		if(numPoints[i] == 0)
			continue;

		vp_x[i] /= numPoints[i];
		vp_y[i] /= numPoints[i];

		// if(numPoints[i] < 1)
		// 	continue;

		considered_idx = i;
		vp.push_back(std::make_pair(vp_x[i], vp_y[i]));

		// cv::circle(colorImg, cv::Point(vp_x[i], vp_y[i]), 3, cv::Scalar(0,0,255),2);
	}

	//cv::imshow("colorImg", colorImg);
	// cv::waitKey();

	// ------------------------------------------------------------------------------------
	// Done. free memory

	for(size_t i = 0 ; i < pts.size() ; ++i) {
		delete pts[i];
	}

	return vp;

}

cv::Mat makeFeatureMap	(std::vector< std::pair<float, float> > &vp, const cv::Mat &img, double *reliability) {
	cv::Mat featureMap(img.rows, img.cols, CV_64FC1, cv::Scalar(0.f));

	if(reliability != NULL)  *reliability = 0.f;

	std::vector<float> dists;
	for(size_t i = 0 ; i < vp.size() ; ++i) {
        float normx = vp[i].first/img.cols;
        float normy = vp[i].second/img.rows;

		if(normx > 1.f || normx < 0.f) continue;
		if(normy > 1.f || normy < 0.f) continue;

        // std::cout << normx << " " << normy << std::endl;

        float d = (normx-.5f)*(normx-.5f) + (normy-.5f)*(normy-.5f);

		dists.push_back(d);
	}

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

    for(size_t i = 0 ; i < vp.size() ; ++i) {
        float normx = vp[i].first/img.cols;
        float normy = vp[i].second/img.rows;

		if(normx > 1.f || normx < 0.f) continue;
		if(normy > 1.f || normy < 0.f) continue;

        // std::cout << normx << " " << normy << std::endl;

        float d = (normx-.5f)*(normx-.5f) + (normy-.5f)*(normy-.5f);

		if(dists.size() > 7 && dists[dists.size()-7] > d) {
			continue;
		}

		if(std::abs(normy-.5f) > 0.25) continue;

		d =  (normy-.5f)*(normy-.5f);

        float sigma = 0.00005f / (d + 0.01f);
		if(reliability != NULL) {
			*reliability = std::max(static_cast<float>(*reliability), sigma);
		}

        for(int i = 0 ; i < img.rows ; ++i) {
            for(int j = 0 ; j < img.cols ; ++j) {
                float fi = static_cast<float>(i) / img.rows;
                float fj = static_cast<float>(j) / img.cols;
                featureMap.at<double>(i,j) = std::max(featureMap.at<double>(i,j), static_cast<double>(std::exp(-((fj-normx)*(fj-normx)*2 + (fi-normy)*(fi-normy))/sigma)));

            }
        }
    }

    return featureMap;
}

cv::Mat vanishingLineFeatureMapF64	(const cv::Mat &img, double *reliability) {

	image_double image_d = new_image_double(img.cols, img.rows);
	for(int i = 0 ; i < img.cols*img.rows ; ++i) {
		image_d->data[i] = reinterpret_cast<double*>(img.data)[i];
	}

	std::vector< std::pair<float, float> > vp = extractVP(image_d, img);

    return makeFeatureMap(vp, img, reliability);
}

cv::Mat vanishingLineFeatureMapF64C3(const cv::Mat &img, double *reliability) {

	cv::Mat imgu(img.rows, img.cols, CV_8UC3, cv::Scalar(0.f, 0.f, 0.f));
    for(int i = 0 ; i < img.rows ; ++i) {
        for(int j = 0 ; j < img.cols ; ++j) {
            cv::Point3_<unsigned char> &o = imgu.at< cv::Point3_<unsigned char> >(i,j);
            const cv::Point3_<double> &in = img.at< cv::Point3_<double> >(i,j);

            o.x = static_cast<unsigned char>(in.x*255);
            o.y = static_cast<unsigned char>(in.y*255);
            o.z = static_cast<unsigned char>(in.z*255);
        }
    }

	std::vector< std::pair<float, float> > vp = extractVP(imgu);

    return makeFeatureMap(vp, img, reliability);
}

cv::Mat vanishingLineFeatureMap (const cv::Mat &img, double *reliability) {

	if(img.empty()) return cv::Mat();

    std::vector< std::pair<float, float> > vp = extractVP(img);

    return makeFeatureMap(vp, img, reliability);
}


#include <iostream>
#include <vector>
#include <random>
#include <chrono>
#include <cstdint>
#include <boost/math/special_functions/prime.hpp>

static const std::size_t kNbItems( 1000000 );

namespace method1
{

void removeDuplicates( std::vector<uint32_t> & items )
{
    using namespace boost::math;
    uint64_t checker = 1u; // Be careful of integer overflow here

    std::size_t n = 0;
    for( const uint32_t x : items )
    {
        const uint32_t p = prime( x );
        if ( checker % p )
        {
            checker *= p;
            items[n++] = x;
        }
    }

    items.resize( n );
}

}

namespace method2
{

void removeDuplicates( std::vector<uint32_t> & items )
{
    std::sort( items.begin(), items.end() );
    items.erase( std::unique( items.begin(), items.end() ), items.end() );
}

}

/**
 * @brief main goes here
 */
int main( int argc, char **argv )
{
    std::mt19937 rng;
    rng.seed( time( 0 ) );

    std::uniform_int_distribution<uint32_t> rand( 0, 10 );
    std::vector<uint32_t> v;
    v.reserve( kNbItems );
    for( std::size_t i = 0; i < kNbItems; ++i )
    {
        v.push_back( rand( rng ) );
    }

    // Benchmark
    typedef std::chrono::high_resolution_clock Clock;
    using std::chrono::microseconds;
    using std::chrono::duration_cast;

    std::vector<uint32_t> uniqueItems1( v );
    {
        auto t1 = Clock::now();
        method1::removeDuplicates( uniqueItems1 );
        auto t2 = Clock::now();
        std::cout << "Method1 took : " << duration_cast<microseconds>(t2-t1).count() << std::endl;
    }

    std::vector<uint32_t> uniqueItems2( v );
    {
        auto t1 = Clock::now();
        method2::removeDuplicates( uniqueItems2 );
        auto t2 = Clock::now();
        std::cout << "Method2 took : " << duration_cast<microseconds>(t2-t1).count() << std::endl;
    }

    std::sort( uniqueItems1.begin(), uniqueItems1.end() );
    std::sort( uniqueItems2.begin(), uniqueItems2.end() );
    if ( uniqueItems1 == uniqueItems2 )
    {
        std::cout << "RESULTS ARE CONFORM" << std::endl;
    }
    else
    {
        std::cerr << "OUPS... Algorithm outputs different resutls!" << std::endl;
    }
    std::cout << "uniqueItems1.size(): " << uniqueItems1.size() << std::endl;
    std::cout << "uniqueItems2.size(): " << uniqueItems2.size() << std::endl;
}


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;

int main( int argc, char **argv )
{
    if ( argc < 2 ){
        std::cout << argv[ 0 ] << " number" << std::endl;

    }else{
        mp::cpp_int number( argv[ 1 ] );

        mp::cpp_int prev_00(1);
        mp::cpp_int prev_01(0);
        mp::cpp_int current(1);
        mp::cpp_int result(0);
        mp::cpp_int i(0);

        while( i < number )
        {
            result = current;
            current = prev_00 + prev_01;
            prev_01 = prev_00;
            prev_00 = current;
            i += 1;
        }
        std::cout << result << std::endl;
    }
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "./include/prints.hpp"

std::vector<int> baseIndex;

void triangularize(Eigen::MatrixXd& B){
    int _m = B.rows();
    int _n = B.cols();
    int start_row = 0;                                  // 每次从start_row开始判定
    for (int col = 0; col < _n; col ++){
        int row = start_row;
        bool push_flag = false;
        if (B(row, col) == 0.0){
            for (; row < _m; row ++){
                if (B(row, col) != 0.0){
                    B.row(col).swap(B.row(row));        // 为0和不为0行进行交换
                    push_flag = true;
                    row ++;
                    break;
                }
            }
        }
        else{
            row ++;
            push_flag = true;
        }
        for (; row < _m; row ++){
            double head = B(row, col);
            if (head != 0.0){
                B.row(row) -= head / B(col, col) * B.row(col);  // 消除头部0
            }
        }
        if (push_flag == true){
            baseIndex.emplace_back(col);
            start_row ++;
            if (start_row >= _m){
                return;
            }
        }
    }
}

int main(){
    std::cout << "==================== test ladderize ==================\n";
    int row = 3, col = 6;
    Eigen::MatrixXd mat(row, col);
    mat << 1, -2, 1, 0, 0, 2,
        0, 1, -3, 1, 0, 1,
        0, 1, -1, 0, 1, 2;
    std::cout << "Before ladderize:\n";
    printMat(mat);
    triangularize(mat);
    std::cout << "After ladderize:\n";
    printMat(mat);
    std::cout << "Base index:\n";
    for (int ind: baseIndex){
        std::cout << ind << ", ";
    }
    std::cout << std::endl;
    Eigen::MatrixXd B(row, row);
    for (int i = 0; i < row; i++){
        B.col(i) = mat.col(baseIndex[i]);
    }
    std::cout << "B:\n";
    printMat(B);
    Eigen::MatrixXd Binv = B.colPivHouseholderQr().solve(Eigen::MatrixXd::Identity(row, row));
    std::cout << "Binv:\n";
    printMat(Binv);
    Eigen::MatrixXd res = Binv * mat;
    std::cout << "Result:\n";
    printMat(res);
    return 0;
}


//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_GIL_IMAGE_PROCESSING_HISTOGRAM_EQUALIZATION_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HISTOGRAM_EQUALIZATION_HPP

#include <boost/gil/histogram.hpp>
#include <boost/gil/image.hpp>

#include <cmath>
#include <map>
#include <vector>

namespace boost { namespace gil {

/////////////////////////////////////////
/// Histogram Equalization(HE)
/////////////////////////////////////////
/// \defgroup HE HE
/// \brief Contains implementation and description of the algorithm used to compute
///        global histogram equalization of input images.
///
///        Algorithm :-
///        1. If histogram A is to be equalized compute the cumulative histogram of A.
///        2. Let CFD(A) refer to the cumulative histogram of A
///        3. For a uniform histogram A', CDF(A') = A'
///        4. We need to transfrom A to A' such that
///        5. CDF(A') = CDF(A) => A' = CDF(A)
///        6. Hence the pixel transform , px => histogram_of_ith_channel[px].
///

/// \fn histogram_equalization
/// \ingroup HE
/// \tparam SrcKeyType Key Type of input histogram
/// @param src_hist INPUT Input source histogram
/// \brief Overload for histogram equalization algorithm, takes in a single source histogram
///        and returns the color map used for histogram equalization.
///
template <typename SrcKeyType>
std::map<SrcKeyType, SrcKeyType> histogram_equalization(histogram<SrcKeyType> const& src_hist)
{
    histogram<SrcKeyType> dst_hist;
    return histogram_equalization(src_hist, dst_hist);
}

/// \overload histogram_equalization
/// \ingroup HE
/// \tparam SrcKeyType Key Type of input histogram
/// \tparam DstKeyType Key Type of output histogram
/// @param src_hist INPUT source histogram
/// @param dst_hist OUTPUT Output histogram
/// \brief Overload for histogram equalization algorithm, takes in both source histogram &
///        destination histogram and returns the color map used for histogram equalization
///        as well as transforming the destination histogram.
///
template <typename SrcKeyType, typename DstKeyType>
std::map<SrcKeyType, DstKeyType>
    histogram_equalization(histogram<SrcKeyType> const& src_hist, histogram<DstKeyType>& dst_hist)
{
    static_assert(
        std::is_integral<SrcKeyType>::value &&
        std::is_integral<DstKeyType>::value,
        "Source and destination histogram types are not appropriate");

    using value_t = typename histogram<SrcKeyType>::value_type;
    dst_hist.clear();
    double sum          = src_hist.sum();
    SrcKeyType min_key  = std::numeric_limits<DstKeyType>::min();
    SrcKeyType max_key  = std::numeric_limits<DstKeyType>::max();
    auto cumltv_srchist = cumulative_histogram(src_hist);
    std::map<SrcKeyType, DstKeyType> color_map;
    std::for_each(cumltv_srchist.begin(), cumltv_srchist.end(), [&](value_t const& v) {
        DstKeyType trnsfrmd_key =
            static_cast<DstKeyType>((v.second * (max_key - min_key)) / sum + min_key);
        color_map[std::get<0>(v.first)] = trnsfrmd_key;
    });
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        dst_hist[color_map[std::get<0>(v.first)]] += v.second;
    });
    return color_map;
}

/// \overload histogram_equalization
/// \ingroup HE
/// @param src_view  INPUT source image view
/// @param dst_view  OUTPUT Output image view
/// @param bin_width INPUT Histogram bin width
/// @param mask      INPUT Specify is mask is to be used
/// @param src_mask  INPUT Mask vector over input image
/// \brief Overload for histogram equalization algorithm, takes in both source & destination
///        image views and histogram equalizes the input image.
///
template <typename SrcView, typename DstView>
void histogram_equalization(
    SrcView const& src_view,
    DstView const& dst_view,
    std::size_t bin_width = 1,
    bool mask = false,
    std::vector<std::vector<bool>> src_mask = {})
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();

    static_assert(
        color_spaces_are_compatible<
            typename color_space_type<SrcView>::type,
            typename color_space_type<DstView>::type>::value,
        "Source and destination views must have same color space");

    // Defining channel type
    using source_channel_t = typename channel_type<SrcView>::type;
    using dst_channel_t    = typename channel_type<DstView>::type;
    using coord_t          = typename SrcView::x_coord_t;

    std::size_t const channels = num_channels<SrcView>::value;
    coord_t const width        = src_view.width();
    coord_t const height       = src_view.height();
    std::size_t pixel_max      = std::numeric_limits<dst_channel_t>::max();
    std::size_t pixel_min      = std::numeric_limits<dst_channel_t>::min();

    for (std::size_t i = 0; i < channels; i++)
    {
        histogram<source_channel_t> h;
        fill_histogram(nth_channel_view(src_view, i), h, bin_width, false, false, mask, src_mask);
        h.normalize();
        auto h2 = cumulative_histogram(h);
        for (std::ptrdiff_t src_y = 0; src_y < height; ++src_y)
        {
            auto src_it = nth_channel_view(src_view, i).row_begin(src_y);
            auto dst_it = nth_channel_view(dst_view, i).row_begin(src_y);
            for (std::ptrdiff_t src_x = 0; src_x < width; ++src_x)
            {
                if (mask && !src_mask[src_y][src_x])
                    dst_it[src_x][0] = channel_convert<dst_channel_t>(src_it[src_x][0]);
                else
                    dst_it[src_x][0] = static_cast<dst_channel_t>(
                        h2[src_it[src_x][0]] * (pixel_max - pixel_min) + pixel_min);
            }
        }
    }
}

}}  //namespace boost::gil

#endif


#include "layer.h"

#include <random>
#include <cmath>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/for_each.hpp>

namespace ann {

Layer::Layer(size_t inputRowCount, size_t outputRowCount, layerFx forwardFunc, layerFx derivateFunc)
    : mForwardFunc(forwardFunc)
    , mDerivateFunc(derivateFunc)
{
    mWeights = math::make_matrix<float>(outputRowCount, inputRowCount);
    mBias = math::make_matrix<float>(outputRowCount, 1);

    std::random_device rd{};
    std::mt19937 generator{rd()};
    std::normal_distribution<float> distribution{0.0f, 1.0f};

    const auto div = std::sqrtf(static_cast<float>(inputRowCount / 2));
    boost::for_each(mWeights, [&generator, &distribution, &div](float &value) {
        value = distribution(generator) / div;
    });
    boost::fill(mBias, 0.01f);
}

math::MatrixF Layer::feedForward(const math::MatrixF &inputData)
{
    mInput = inputData;
    mZ = (mWeights * mInput).addColumnVector(mBias);
    mActivation = mForwardFunc(mZ);
    return mActivation;
}

math::MatrixF Layer::beginBackPropagation(const math::MatrixF &expectedOutput)
{
    const auto m = static_cast<float>(mInput.getColumnCount());

    auto lossd = mActivation - expectedOutput;
    mWeightDelta = (1.0f / m) * (lossd * mInput.transpose());
    mBiasDelta =  (1.0f / m) * math::rowSum(lossd);

    return mWeights.transpose() * lossd;
}

math::MatrixF Layer::calculateGradients(const math::MatrixF& prevDz)
{
    const auto m = static_cast<float>(mInput.getColumnCount());

    const auto dz = prevDz.hadamardProduct(mDerivateFunc(mActivation));
    mWeightDelta = (1.0f / m) * (dz * mInput.transpose());
    mBiasDelta = (1.0f / m) * math::rowSum(dz);

    return mWeights.transpose() * dz;
}

void Layer::applyGradients(float learnSpeed)
{
    mWeights = mWeights - (learnSpeed * mWeightDelta);
    mBias = mBias - (learnSpeed * mBiasDelta);
}

}