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

/*
 Copyright (C) 2010, Sebastian Schlenkrich

*/

/*! \file auxilliariesT.hpp
    \brief provide template functions for required computations with active data types
*/

#ifndef quantlib_templateauxilliaries_hpp
#define quantlib_templateauxilliaries_hpp

#include <boost/math/special_functions/erf.hpp>
#include <ql/experimental/templatemodels/auxilliaries/minimADVariable2T.hpp>
#include <ql/experimental/templatemodels/auxilliaries/integratorsT.hpp>

namespace TemplateAuxilliaries {

    inline double erf(const double x)     { return boost::math::erf(x); }
    inline double erf_inv(const double x) { return boost::math::erf_inv(x); }
    inline double DBL(const double x)     { return x; }
//	inline double DBL(const ADTAGEO::daglad x) { return x.val(); }
    inline double DBL(const MinimAD::Variable<QuantLib::Real> x) { return x.value(); }

    // transformation (-inf, +inf) -> (a, b)
    template <typename Type> inline Type direct(const Type x, const Type a, const Type b) {
        Type y = (x<0.0) ? (-1.0/(x-1.0)) : (-1.0/(x+1.0)+2.0);
        y = 0.5*(b-a)*y + a;
        return y;
    }

    // transformation (a, b) -> (-inf, +inf)
    template <typename Type> inline Type inverse(const Type y, const Type a, const Type b) {
        Type x = 2.0*(y-a)/(b-a);
        x = (x<1.0) ? (-1.0/x+1.0) : (-1.0/(x-2.0)-1.0);
        return x;
    }

    // square of vector elements
    template <typename Type> inline
    std::vector<Type> sqr(std::vector<Type> x) {
        std::vector<Type> x2(x.size());
        for (size_t i=0; i<x.size(); ++i) x2[i] = x[i]*x[i];
        return x2;
    }

    // find index in ascending vector
    // evaluate n s.t. t[n-1] < t <= t[n]
    template <typename Type> inline
    size_t idx(const std::vector<Type>& times, const Type t) {
        if ((t <= times[0]) | (times.size()<2)) return 0;
        if (t >  times[times.size()-2 ])        return times.size()-1;
        // bisection search
        size_t a = 0, b = times.size()-2;
        while (b-a>1) {
            size_t s = (a + b) / 2;
            if (t <= times[s]) b = s;
            else                a = s;
        }
        return b;
    }

    // union of two vectors
    template <typename Type> inline
    std::vector<Type> unionVector(const std::vector<Type>& v1, const std::vector<Type>& v2) {
        std::vector<Type> res;
        std::set_union(v1.begin(), v1.end(), v2.begin(), v2.end(), std::back_inserter(res));
        return res;
    }

    //  normal distribution and Black76 functions

    template <typename Type> inline
    Type Phi( Type x) {
        return 0.5*(erf(x*M_SQRT1_2)+1.0);
    }

    template <typename Type> inline
    Type phi( Type x) {
        return M_SQRT1_2 * M_1_SQRTPI * exp(-x*x/2.0);
    }

    template <typename Type> inline
    Type PhiInv(const Type x) {
        return M_SQRT2*erf_inv(2*x-1);
    }

    template <typename Type> inline
    Type Black76(Type F,        // forward price
                 Type K,        // strike
                 Type sigma,    // Black76 volatility
                 Type T,        // Time to expiry
                 int  cop       // call (1) or put (-1) option
                ) {
        Type d1 = log(F/K)/sigma/sqrt(T) + sigma*sqrt(T)/2.0;
        Type d2 = d1 - sigma*sqrt(T);
        return cop * (F*Phi(cop*d1) - K*Phi(cop*d2));
    }

    template <typename Type> inline
    Type Black76Vega(Type F,        // forward price
                    Type K,        // strike
                    Type sigma,    // Black76 volatility
                    Type T,        // Time to expiry
                    int    cop     // call (1) or put (-1) option
                    ) {
        Type d1 = log(F/K)/sigma/sqrt(T) + sigma*sqrt(T)/2.0;
        return F * exp(-d1*d1/2.0) * sqrt( T / 2.0 / M_PI);
    }

    template <typename Type> inline
    Type Bachelier(Type F,        // forward price
                   Type K,        // strike
                   Type sigma,    // Normal volatility
                   Type T,        // Time to expiry
                   int  cop       // call (1) or put (-1) option
                ) {
        Type d = cop*(F-K);
        Type h = d/sigma/sqrt(T);
        return d*Phi(h) + phi(h)*sigma*sqrt(T);
    }

    //  cubic interpolation and integration of expectation

    //  solve A X = Y by LU decomposition where A = tridiag { a, b, c }
    template <typename DateType, typename ValueType> inline void
    solveTridiagLinearSystem ( std::vector<DateType>&   a,  // input: a_1 to a_{dim-1}, output: l_1 to l_{dim-1} of L
                               std::vector<DateType>&   b,  // input: b_0 to b_{dim-1}, output: u_0 to u_{dim-1} of U
                               std::vector<DateType>&   c,  // input: c_0 to c_{dim-2}, output: v_0 to v_{dim-2} of U
                               std::vector<ValueType>&  y,  // input: right hand sides y, output: solutions x
                               std::vector<ValueType>&  z   // intermediates
                               ) {
        size_t dim = b.size();
        // in place LU decomposition; no error handling if LU decomposition does not exist
        for (size_t i=1; i<dim; ++i) {
            a[i] /= b[i-1];
            b[i] -= c[i-1]*a[i];
        }
        // forward substitution
        z[0] = y[0];
        for (size_t i=1; i<dim; ++i) z[i] = y[i] - a[i]*z[i-1];
        // backward substitution, eliminate input
        y[dim-1] = z[dim-1]/b[dim-1];
        //for (long i=dim-2; i>=0; --i) y[i] = (z[i] - c[i]*y[i+1])/b[i];
        for (size_t i=dim-1; i>0; --i) y[i-1] = (z[i-1] - c[i-1]*y[i])/b[i-1];
    }

    //  (Log) Cubic interpolation requires precomputed derivatives g[i] = y'[i]
    template <typename DateType, typename ValueType> inline
    void c2splineDerivatives( std::vector<DateType>&  x,  // input parameter, strictly increasing grid point
                              std::vector<ValueType>& y,  // input parameter, function values at x[i]
                              std::vector<ValueType>&       g,  // output parameter, derivatives at x[i]
                              std::vector<ValueType>&       z,  // intermediates
                              int       logInterpolation  = 0,  // (1) interpolate log(y[i]), (0) standard
                              int       boundaryCondition = 1   // (0) g[0] = g[dim-1] = 0,
                                                                // (1) g[0], g[dim-1] by secants
                        ) {
        ValueType dy1, dy2;
        size_t i, dim = std::min(x.size(),y.size());
        std::vector<DateType> a(dim), b(dim), c(dim);
        // constant extrapolation
        if (dim==1) {
            g[0] = 0.0;
        }
        // see Wikipedia 'spline interpolation'
        for (i=1; i<dim-1; ++i) {
            a[i] = x[i+1] - x[i];
            b[i] = 2.0*(x[i+1] - x[i-1]);
            c[i] = x[i] - x[i-1];
            dy1  = (logInterpolation) ? log(y[i]/y[i-1]) : y[i] - y[i-1];
            dy2  = (logInterpolation) ? log(y[i+1]/y[i]) : y[i+1] - y[i];
            g[i] = 3.0*(a[i]/c[i]*dy1 + c[i]/a[i]*dy2);
            dy1 = 0.0; dy2 = 0.0; // elimination
        }
        // boundary conditions
        a[0] = 0.0; c[0] = 0.0; g[0] = 0.0; a[dim-1] = 0.0; c[dim-1] = 0.0; g[dim-1] = 0.0;
        b[0] = 1.0; b[dim-1] = 1.0;
        if (boundaryCondition && (dim>1)) { // fix boundaries
            g[0] =  (logInterpolation) ? log(y[1]/y[0]) : (y[1]-y[0]);
            g[0] /= (x[1]-x[0]);
            g[dim-1] =  (logInterpolation) ? log(y[dim-1]/y[dim-2]) : (y[dim-1]-y[dim-2]);
            g[dim-1] /= (x[dim-1]-x[dim-2]);
        }
        // solve tridiag [ a, b, c ] x = g and x -> g
        solveTridiagLinearSystem( a, b, c, g, z );
    }

    template <typename DateType, typename ValueType> inline
    ValueType interpolCSpline ( DateType                      xi,
                                std::vector<DateType>&  x,
                                std::vector<ValueType>& y,
                                std::vector<ValueType>& g,
                                int    logInterpolation = 0) {
        size_t prevIdx, nextIdx, idx;
        size_t dim = std::min(x.size(),y.size());
        dim = std::min(dim,g.size());
        ValueType h, u, v, p, q, yPrev, yNext, res;
        // linear extrapolation
        if (xi<=x[0]) {
            res = (logInterpolation) ? log(y[0]) : y[0];
            res += g[0]*(xi-x[0]);
            res = (logInterpolation) ? exp(res) : res;
            return res;
        }
        if (xi>=x[dim-1]) {
            res = (logInterpolation) ? log(y[dim-1]) : y[dim-1];
            res += g[dim-1]*(xi-x[dim-1]);
            res = (logInterpolation) ? exp(res) : res;
            return res;
        }
        // find prevIdx and nextIdx s.t. x[prevIdx] < xi <= x[nextIdx]
        prevIdx = 0;
        nextIdx = dim-1;
        while (nextIdx>prevIdx+1) {
            idx = (prevIdx + nextIdx)/2;
            if (xi<=x[idx]) nextIdx = idx;
            else            prevIdx = idx;
        }
        // auxilliary variables
        h = x[nextIdx] - x[prevIdx];
        u = (xi - x[prevIdx])/h;
        v = 1.0 - u;
        yPrev = (logInterpolation) ? log(y[prevIdx]) : y[prevIdx];
        yNext = (logInterpolation) ? log(y[nextIdx]) : y[nextIdx];
        p = 3.0*yNext - h*g[nextIdx];
        q = 3.0*yPrev + h*g[prevIdx];
        // final interpolation
        res = yNext*u*u*u + p*u*u*v + q*u*v*v + yPrev*v*v*v;
        res = (logInterpolation) ? exp(res) : res;
        return res;
    }

    // integration x_0, ..., x_N plus extrapolation via Bachelier formula
    template <typename PassiveType, typename ActiveType> inline
    ActiveType normalExpectation(
                std::vector<PassiveType>&   x,    //  grid points of payoff
                std::vector<ActiveType>&    v,    //  payoff
                std::vector<ActiveType>&    g,  //  derivatives of payoff for interpolation
                ActiveType                  mu,   //  expectation of normal distribution
                ActiveType                  var,  //  variance sigma^2 of normal distribution
                std::string                 method="" //  integration method
                  ) {
        ActiveType res=0.0;
        if (x.size()==0) return res;

        // low rate extrapolation
        ActiveType Q0 = Phi((x[0]-mu)/sqrt(var));
        res = v[0]*Q0;
        if (x.size()==1) return res;
        res -= (v[1]-v[0])/(x[1]-x[0])*Bachelier(mu,(ActiveType)x[0],sqrt(var),(ActiveType)(1.0),-1);
        //res -= g[0]*Bachelier(mu,(ActiveType)x[0],sqrt(var),(ActiveType)(1.0),-1);

        // high rate extrapolation
        ActiveType QN = Phi((x[x.size()-1]-mu)/sqrt(var));
        res += v[v.size()-1]*(1.0-QN);
        res += (v[v.size()-1]-v[v.size()-2])/(x[x.size()-1]-x[x.size()-2])*Bachelier(mu,(ActiveType)x[x.size()-1],sqrt(var),(ActiveType)(1.0),+1);
        //res += g[x.size()-1]*Bachelier(mu,(ActiveType)x[x.size()-1],sqrt(var),(ActiveType)(1.0),+1);

        // switch methods...

        // default intervall integrations
        // boundaries
        res += v[v.size()-1]*QN - v[0]*Q0;
        // replication via Put-Spreads
        ActiveType B1, B2 = Bachelier(mu,(ActiveType)x[0],sqrt(var),(ActiveType)(1.0),-1);
        for (size_t i=0; i<x.size()-1; ++i) {
            B1  = B2;
            B2  = Bachelier(mu,(ActiveType)x[i+1],sqrt(var),(ActiveType)(1.0),-1);
            res -= (v[i+1] - v[i]) / (x[i+1] - x[i]) * (B2 - B1);
            //res -= 0.5 * (g[i] + g[i+1]) * (B2 - B1);
        }
        return res;
    }

    template <typename PassiveType, typename ActiveType> inline
    ActiveType normalExpectation(
                std::vector<PassiveType>&   x,  //  grid points of payoff
                std::vector<ActiveType>&    v,  //  payoff
                std::vector<ActiveType>&    g,  //  derivatives of payoff for interpolation
                ActiveType                       mu,  //  expectation of normal distribution
                ActiveType                      var,  //  variance sigma^2 of normal distribution
                PassiveType                     tol   //  tolerance for accuracy
                  ) {

        if ((std::min(x.size(),v.size())<2)||(x.size()!=v.size())) return 0;
        if ((g.size()<2)||(g.size()!=x.size())) return 0;
        if (tol<=0) return normalExpectation( x, v, g, mu, var );
        PassiveType x0, x1, x2, h, err, tmp, lambda, sum1;
        ActiveType  v0, v1, v2;
        ActiveType  y0, y1, y2, sum2, res;
        std::vector<ActiveType> sums(x.size());
        sums.push_back((ActiveType)0.0);
        for (int i=0; i<2; ++i) {
            x0 = DBL(mu);
            v0 = interpolCSpline(x0,x,v,g);
            y0 = v0 * exp(-(x0-mu)*(x0-mu)/2.0/var);
            //h = (1-2*i)*tol; // first stupid guess
            h = (1-2*i)*x[x.size()-1]/x.size()*2;
            x2 = x0 + h;
            v2 = interpolCSpline(x2,x,v,g);
            y2 = v2 * exp(-(x2-mu)*(x2-mu)/2.0/var);
            // integrate mu to +infty
            tmp = Phi(DBL((x0-mu)/sqrt(var)));
            tmp = (tmp<0.5) ? tmp : (1.0-tmp);
            while ((tmp>tol*tol)||(fabs(DBL(v0))*tmp>tol*tol)) {
                x1 = x0 + 0.5*h;
                v1 = interpolCSpline(x1,x,v,g);
                y1 = v1 * exp(-(x1-mu)*(x1-mu)/2.0/var);
                sum1 = DBL(y0 + y2)*h/2.0;           // order-3
                sum2 = (y0 + 4.0*y1 + y2)*h/6.0;  // order-5
                err = fabs((sum1-DBL(sum2))/h);
                if (err>tol) {  // reject the step
                    h *= 0.5;
                    x2 = x1;
                    v2 = v1;
                    y2 = y1;
                    continue;  // try again with half the step size
                }
                //res += (1-2*i)*sum2;  // use the more accurate estimate
                sums.push_back(sums.back() + (1-2*i)*sum2);
                x0 = x2;
                v0 = v2;
                y0 = y2;
                lambda = sqrt(tol/(err+1.0e-32));
                lambda = (lambda>2.0) ? 2.0 : lambda; // cap step size increase
                lambda = ((lambda>1.0)&(lambda<1.3)) ? 1.0 : lambda; // avoid step size oscillation
                h *= lambda;
                x2 = x0 + h;
                v2 = interpolCSpline(x2,x,v,g);
                y2 = v2 * exp(-(x2-mu)*(x2-mu)/2.0/var);
                tmp = Phi(DBL((x0-mu)/sqrt(var)));
                tmp = (tmp<0.5) ? tmp : (1.0-tmp);
            }
        }
        // don't forget the scaling factor...
        res = sums.back() / sqrt(2.0 * M_PI * var);
        // reverse elimination
        for (size_t i=sums.size(); i>0; --i) sums[i-1] = 0.0;
        return res;
    }

}

#endif  /* quantlib_templateauxilliaries_hpp */


#pragma once

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

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between a two infinite lines in 3D.
/// @note The distance is actually squared distance.
/// @warning If the lines are parallel this function returns a distance of zero.
/// @param ea0 The first vertex of the edge defining the first line.
/// @param ea1 The second vertex of the edge defining the first line.
/// @param ea0 The first vertex of the edge defining the second line.
/// @param ea1 The second vertex of the edge defining the second line.
/// @return The distance between the two lines.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
auto line_line_distance(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    const auto normal = cross(ea1 - ea0, eb1 - eb0);
    const auto line_to_line = (eb0 - ea0).dot(normal);
    return line_to_line * line_to_line / normal.squaredNorm();
}

// Symbolically generated derivatives;
namespace autogen {
    void line_line_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12]);

    void line_line_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a two lines in 3D.
/// @note The distance is actually squared distance.
/// @warning If the lines are parallel this function returns a distance of zero.
/// @param[in] ea0 The first vertex of the edge defining the first line.
/// @param[in] ea1 The second vertex of the edge defining the first line.
/// @param[in] ea0 The first vertex of the edge defining the second line.
/// @param[in] ea1 The second vertex of the edge defining the second line.
/// @param[out] hess The gradient of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedGrad>
void line_line_distance_gradient(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    grad.resize(ea0.size() + ea1.size() + eb0.size() + eb1.size());
    autogen::line_line_distance_gradient(
        ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2],
        eb1[0], eb1[1], eb1[2], grad.data());
}

/// @brief Compute the hessian of the distance between a two lines in 3D.
/// @note The distance is actually squared distance.
/// @warning If the lines are parallel this function returns a distance of zero.
/// @param[in] ea0 The first vertex of the edge defining the first line.
/// @param[in] ea1 The second vertex of the edge defining the first line.
/// @param[in] ea0 The first vertex of the edge defining the second line.
/// @param[in] ea1 The second vertex of the edge defining the second line.
/// @param[out] hess The hessian of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedHess>
void line_line_distance_hessian(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    hess.resize(
        ea0.size() + ea1.size() + eb0.size() + eb1.size(),
        ea0.size() + ea1.size() + eb0.size() + eb1.size());
    autogen::line_line_distance_hessian(
        ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2],
        eb1[0], eb1[1], eb1[2], hess.data());
}

} // namespace ipc


/*

Defines the functions of the Frame

Author  : Subramanian Krishnan
Date    : 30 Sep 2020

Changelog:
    subbu - 9/30  - Initial commit
    subbu - 10/27 - update members+methods
*/

#include <iostream>
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>

// Custom headers
#include "frame.h"
#include "utils.h"
#include "optim.h"

void FramePair::SampleIndices(const Eigen::MatrixXi& lines, std::vector<points2d>& sampled_lines_2d){
    /*
    Input: lines matrix expected to be of form nx4 with each entry being [x1,y1,x2,y2]
           where x_i, y_i denote the end points of the lines

    Output: Sampled points of the form (2n)x(n_samples) vector.
    note: different lines have different n_samples based on line length
    */

    const int max_samples = 100;

    // Iterating through row elements is possible in eigen 3.4
    for(int i = 0; i < lines.rows(); i++ ){
        Eigen::Vector2i p1(lines(i,0), lines(i,1)), p2(lines(i,2), lines(i,3));
        float dist = (p1-p2).norm();
        float n_samples = std::min(max_samples, int(dist));
        u_int point_index = 0;

        // ref: https://stackoverflow.com/questions/28018147/emgucv-get-coordinates-of-pixels-in-a-line-between-two-points
        int x0 = lines(i,0), y0 = lines(i,1), x1 = lines(i,2), y1 = lines(i,3);
        int dx = std::abs(x1- x0), dy = std::abs(y1-y0);
        int sx = x0 < x1 ? 1 : -1;
        int sy = y0 < y1 ? 1 : -1;
        int err = dx - dy;

        // For each line store the index points (need to be 'int')
        Eigen::Matrix<int, 2, max_samples> line_points;

        if (n_samples < max_samples){
            // Bresenham's algorithm
            while(true){
                line_points(0, point_index) = x0;
                line_points(1, point_index) = y0;
                point_index++;

                if(x0 == x1 && y0 == y1) break;
                int e2 = 2*err;
                if(e2 > -dy){
                    err = err - dy;
                    x0 = x0 + sx;
                }
                if(e2 < dx){
                    err = err + dx;
                    y0 = y0 + sy;
                }
            }

        }
        else{
         for (int i=1; i<=max_samples; ++i){
            line_points(0, point_index) = x0 + (x1 - x0)*(i-1)/(max_samples-1);
            line_points(1, point_index) = y0 + (y1 - y0)*(i-1)/(max_samples-1);
            point_index++;
         }
        }
        // Add the x and y indices to class variable
        sampled_lines_2d.push_back(line_points.leftCols(point_index));
    }
}

Eigen::Matrix3d FramePair::Cov3D(double u, double v, double depth){
    // depth noise co-efficients for the kinect [ref: Lu, Song 2015]
    const double c1 = 0.00273, c2 = 0.00074, c3 = -0.00058;
    // Our estiamte of how accurate the 2D point sampled on a line is (Not sure if this needs to be changed. May require tuning)
    const double sigma_g = 1;
    double sigma_d = c1*depth*depth + c2*depth + c3;

    Eigen::Matrix3d cov2d = (Eigen::Matrix<double,3,3>()<<
                             sigma_g*sigma_g, 0,               0,
                             0,               sigma_g*sigma_g, 0,
                             0,               0,               sigma_d*sigma_d).finished();

    Eigen::Matrix3d J = (Eigen::Matrix<double,3,3>()<<
                         depth/K(0,0), 0,            (u - K(0,2))/K(0,0),
                         0,            depth/K(1,1), (v - K(1,2))/K(1,1),
                         0,            0,            1).finished();

    Eigen::Matrix3d cov3d = J*cov2d*(J.transpose());

    return cov3d;
}

void FramePair::Reproject(){

    std::size_t num_lines = sampled_lines_2d_im1.size();
    int l1_num_points, l2_num_points;

    for(int i=0; i < num_lines; ++i){
        std::vector<Eigen::Matrix3d> l1_cov, l1_eig_vec, l2_cov, l2_eig_vec, l1_inv_cov, l2_inv_cov;
        std::vector<Eigen::Vector3d> l1_eig_val, l2_eig_val;

        Eigen::Matrix3Xd P1(3, sampled_lines_2d_im1[i].cols()), P2(3, sampled_lines_2d_im2[i].cols());
        l1_num_points = reprojectSingleLine(depth_image1, sampled_lines_2d_im1[i], P1, l1_cov, l1_eig_val, l1_eig_vec, l1_inv_cov);
        l2_num_points = reprojectSingleLine(depth_image2, sampled_lines_2d_im2[i], P2, l2_cov, l2_eig_val, l2_eig_vec, l2_inv_cov);

        if(l1_num_points and l2_num_points){
            // Update line 1 details
            points_3d_im1.push_back(P1.leftCols(l1_num_points));
            cov_G_im1.push_back(l1_cov);
            cov_eig_values_im1.push_back(l1_eig_val);
            cov_eig_vectors_im1.push_back(l1_eig_vec);
            im1_data.cov_matrices.push_back(l1_inv_cov);

            // Update line 2 details
            points_3d_im2.push_back(P2.leftCols(l2_num_points));
            cov_G_im2.push_back(l2_cov);
            cov_eig_values_im2.push_back(l2_eig_val);
            cov_eig_vectors_im2.push_back(l2_eig_vec);
            im2_data.cov_matrices.push_back(l2_inv_cov);
        }
    }
}

int FramePair::reprojectSingleLine(const cv::Mat& depth_image, const points2d& current_line, points3d& P, std::vector<Eigen::Matrix3d>& line_covariance,
                                   std::vector<Eigen::Vector3d>& eigen_values, std::vector<Eigen::Matrix3d>& eigen_vectors,
                                   std::vector<Eigen::Matrix3d>& inv_cov_one_line){
    int index = 0;
    for(int i=0; i<current_line.cols(); ++i){

        // rerpoject to 3D using depth value
        int u = current_line(0,i), v = current_line(1,i);
        float depth = depth_image.at<uint16_t>(v, u)/5000.0;
        if(depth == 0) continue;
        P(0,index) = (u - K(0,2))*depth/K(0,0);
        P(1,index) = (v - K(1,2))*depth/K(1,1);
        P(2,index) = depth;
        index++;

        // Propogate 2D covariance to 3D
        Eigen::Matrix3d covariance_3d = Cov3D(u, v, depth);

        // Generate Eigen values of cov for ransac later
        Eigen::SelfAdjointEigenSolver<covariance> eigensolver(covariance_3d);
        if (eigensolver.info() != Eigen::Success){
            cout << "Could not perform eigen decomposition on 3D point's covariance matrix" << endl;
            cout << covariance_3d << endl;
            abort();
        }

        Eigen::Matrix3d U = eigensolver.eigenvectors();
        Eigen::Vector3d D = eigensolver.eigenvalues();
        Eigen::Matrix3d CovInvRoot = ((D.array().inverse()).sqrt()).matrix().asDiagonal() * U.transpose();

        line_covariance.push_back(covariance_3d);
        eigen_values.push_back(D);
        eigen_vectors.push_back(U);
        inv_cov_one_line.push_back(CovInvRoot);
    }

    return index >= 5 ? index : 0;
}

FramePair::FramePair(const cv::Mat& rgb_image1, cv::Mat& depth_image1, cv::Mat& rgb_image2, cv::Mat& depth_image2) :    rgb_image1(rgb_image1),
                                                                                depth_image1(depth_image1),
                                                                                rgb_image2(rgb_image2),
                                                                                depth_image2(depth_image2) {
    // Make the intrinsic matrix [Got from dataset information]
    K = (Eigen::Matrix<double, 3, 3>() << 517.306408, 0.000000, 318.643040,
                                            0.000000, 516.469215, 255.313989,
                                            0.000000, 0.000000, 1.000000).finished();

    // populate distortion
    dist = {0.262383, -0.953104, -0.005358, 0.002628, 1.163314};

    // populate image dimensions
    im_wd = rgb_image1.cols; im_ht = rgb_image1.rows;

    // Make ransac object for culling outliers later
    pointRefine = new Ransac(10, 1);

    // Function returing lines in both images and matches between them contained in a structure element
    pstruct = image_process(rgb_image1, rgb_image2);

    img1_lines.resize(int(pstruct.matches.size()/2),4);
    img2_lines.resize(int(pstruct.matches.size()/2),4);

    int lineIDLeft;
    int lineIDRight;
    for (unsigned int pair = 0; pair < pstruct.matches.size() / 2; pair++)
    {
        lineIDLeft = pstruct.matches[2 * pair];
        lineIDRight = pstruct.matches[2 * pair + 1];
        Eigen::Vector4i r1(int(pstruct.linesInLeft[lineIDLeft][0].startPointX), int(pstruct.linesInLeft[lineIDLeft][0].startPointY),
                            int(pstruct.linesInLeft[lineIDLeft][0].endPointX), int(pstruct.linesInLeft[lineIDLeft][0].endPointY));
        Eigen::Vector4i r2(int(pstruct.linesInRight[lineIDRight][0].startPointX), int(pstruct.linesInRight[lineIDRight][0].startPointY),
                            int(pstruct.linesInRight[lineIDRight][0].endPointX), int(pstruct.linesInRight[lineIDRight][0].endPointY));
        img1_lines.row(pair) = r1;
        img2_lines.row(pair) = r2;

        /*
        // ---------Following code allows to visualize individual matches between the two images-------------
        cv::Point startPoint = cv::Point(int(pstruct.linesInLeft[lineIDLeft][0].startPointX), int(pstruct.linesInLeft[lineIDLeft][0].startPointY));
        cv::Point endPoint = cv::Point(int(pstruct.linesInLeft[lineIDLeft][0].endPointX), int(pstruct.linesInLeft[lineIDLeft][0].endPointY));
        cv::line(rgb_image1, startPoint, endPoint, CV_RGB(255,0,0), 1, cv::LINE_AA, 0);
        startPoint = cv::Point(int(pstruct.linesInRight[lineIDRight][0].startPointX), int(pstruct.linesInRight[lineIDRight][0].startPointY));
        endPoint = cv::Point(int(pstruct.linesInRight[lineIDRight][0].endPointX), int(pstruct.linesInRight[lineIDRight][0].endPointY));
        cv::line(rgb_image2, startPoint, endPoint, CV_RGB(255,0,0), 1, cv::LINE_AA, 0);
        utils::DisplayDualImage(rgb_image1, rgb_image2);
        cv::waitKey(0);
        */

    }
    // std::cout << "Im1 line " << img1_lines.rows() << " " << img1_lines.cols() << std::endl;
    // std::cout << "Im2 line " << img2_lines.rows() << " " << img2_lines.cols() << std::endl;
    // sample lines in image
    SampleIndices(img1_lines, sampled_lines_2d_im1);
    SampleIndices(img2_lines, sampled_lines_2d_im2);

    // Reproject left and right image points to 3D
    Reproject();

    // optim::nonlinOptimize(points_3d_im1[0], im1_data.cov_matrices[0], 0, im1_data.cov_matrices[0].size()-1);

    // TODO: CHECK REPROJECT FUNCTION
    // TODO: ASSIGN STRUCT ELEMENTS HERE (HAVE TO OBTAIN IDX1 AND IDX2)

    // cull outlier points
    // std::cout << "C1" << std::endl;
    for(int i=0; i < points_3d_im1.size(); ++i){
        std::vector<Eigen::Matrix3d> updated_covariance;
        std::vector<Eigen::Matrix3d> updated_inv_root_covariance;

        // point update
        rsac_points_3d_im1.push_back(pointRefine->removeOutlierPoints(points_3d_im1[i], cov_eig_values_im1[i], cov_eig_vectors_im1[i],
                                     updated_covariance, updated_inv_root_covariance, cov_G_im1[i], im1_data.cov_matrices[i]));

        // cov update
        cov_G_im1[i] = updated_covariance;
        im1_data.cov_matrices[i] = updated_inv_root_covariance;
    }

    for(int i=0; i < points_3d_im2.size(); ++i){
        std::vector<Eigen::Matrix3d> updated_covariance;
        std::vector<Eigen::Matrix3d> updated_inv_root_covariance;

        // point update
        rsac_points_3d_im2.push_back(pointRefine->removeOutlierPoints(points_3d_im2[i], cov_eig_values_im2[i], cov_eig_vectors_im2[i],
                                    updated_covariance, updated_inv_root_covariance, cov_G_im2[i], im2_data.cov_matrices[i]));

        // cov update
        cov_G_im2[i] = updated_covariance;
        im2_data.cov_matrices[i] = updated_inv_root_covariance;
    }
    // std::cout << "C2" << std::endl;
    // std::cout << rsac_points_3d_im1.size() << std::endl;
    // std::cout << rsac_points_3d_im2.size() << std::endl;
    // std::cout << im1_data.cov_matrices.size() << std::endl;
    // std::cout << im2_data.cov_matrices.size() << std::endl;

    std::cout << "Starting Im1" << std::endl;
    int Inp;

    std::vector<Eigen::Matrix3d> line1_endPt_covs, line2_endPt_covs;

    for(int i = 0; i < rsac_points_3d_im1.size(); i++){
    // std::cout << rsac_points_3d_im1[i].cols() << " " << im1_data.cov_matrices[i].size() << std::endl;
    points3d optimized_line1 = optim::nonlinOptimize(rsac_points_3d_im1[i], im1_data.cov_matrices[i], cov_G_im1[i], line1_endPt_covs, 0, im1_data.cov_matrices[i].size()-1);
    optimized_lines_im1.push_back(optimized_line1);
    // std::cin >> Inp;
    }

    std::cout << "Starting Im2" << std::endl;
    for(int i = 0; i < rsac_points_3d_im2.size(); i++){
    points3d optimized_line2 = optim::nonlinOptimize(rsac_points_3d_im2[i], im2_data.cov_matrices[i], cov_G_im2[i], line2_endPt_covs, 0, im2_data.cov_matrices[i].size()-1);
    optimized_lines_im2.push_back(optimized_line2);
    }
    std::cout << "Matrix Size" << line1_endPt_covs.size() << " " << line2_endPt_covs.size() << std::endl;

}

    // points3d FramePair::OptimizeFrames(){

    // }


/*JULKA - Julka
#big-numbers

Julka surprised her teacher at preschool by solving the following riddle:

Klaudia and Natalia have 10 apples together, but Klaudia has two apples more than Natalia. How many apples does each of he girls have?

Julka said without thinking: Klaudia has 6 apples and Natalia 4 apples. The teacher tried to check if Julka's answer wasn't accidental and repeated the riddle every time increasing the numbers. Every time Julka answered correctly. The surprised teacher wanted to continue questioning Julka, but with big numbers she could't solve the riddle fast enough herself. Help the teacher and write a program which will give her the right answers.
Task

Write a program which

    reads from standard input the number of apples the girls have together and how many more apples Klaudia has,
    counts the number of apples belonging to Klaudia and the number of apples belonging to Natalia,
    writes the outcome to standard output

Input

Ten test cases (given one under another, you have to process all!). Every test case consists of two lines. The first line says how many apples both girls have together. The second line says how many more apples Klaudia has. Both numbers are positive integers. It is known that both girls have no more than 10100 (1 and 100 zeros) apples together. As you can see apples can be very small.
Output

For every test case your program should output two lines. The first line should contain the number of apples belonging to Klaudia. The second line should contain the number of apples belonging to Natalia.
Example

Input:
10
2
[and 9 test cases more]

Output:
6
4
[and 9 test cases more]

*/

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

int main()
{
    using namespace boost::multiprecision;

    for (int n = 0; n < 10; ++n)
    {
        cpp_int a, b, c;

        if (std::cin >> a >> b)
        {
            c = (a - b) / 2;
            std::cout << c + b << "\n" << c << std::endl;
        }
    }

    return 0;
}


/*! \file helium_hf.cpp
    \brief Hartree-Fock法で、ヘリウム原子のエネルギーを計算する
    Copyright © 2017 @dc1394 All Rights Reserved.
    (but this is originally adapted by Paolo Giannozzi for helium_hf_gauss.c from http://www.fisica.uniud.it/~giannozz/Corsi/MQ/Software/C/helium_hf_gauss.c )
    This software is released under the BSD 2-Clause License.
*/

#include <chrono>                               // for std::chrono
#include <cmath>                                // for std::pow, std::sqrt
#include <cstdint>                              // for std::int32_t
#include <iostream>                             // for std::cerr, std::cin, std::cout
#include <optional>                             // for std::make_optional, std::optional, std::nullopt
#include <valarray>                             // for std::valarray
#include <boost/assert.hpp>                     // for BOOST_ASSERT
#include <boost/format.hpp>                     // for boost::format
#include <boost/math/constants/constants.hpp>   // for boost::math::constants::pi
#include <boost/multi_array.hpp>                // for boost::multi_array
#include <Eigen/Core>                           // for Eigen::MatrixXd, Eigen::VectorXd
#include <Eigen/Eigenvalues>                    // for Eigen::GeneralizedSelfAdjointEigenSolver

namespace {
    //! A global variable (constant expression).
    /*!
        バッファサイズの上限
    */
    static auto constexpr MAXBUFSIZE = 32;

    //! A global variable (constant expression).
    /*!
        SCF計算のループの上限
    */
	static auto constexpr MAXITER = 1000;

    //! A global variable (constant expression).
    /*!
        SCF計算のループから抜ける際のエネルギーの差の閾値
    */
	static auto constexpr SCFTHRESHOLD = 1.0E-15;

    //! A global function.
    /*!
        SCF計算を行う
        \return SCF計算が正常に終了した場合はエネルギーを、しなかった場合はstd::nulloptを返す
    */
    std::optional<double> do_scfloop();

    //! A global function.
    /*!
        nalpha個のGTOによるヘリウム原子のエネルギーを計算する
        \param c 固有ベクトルC
        \param ep 一般化固有値問題のエネルギー固有値E'
        \param h 1電子積分
        \return ヘリウム原子のエネルギー
    */
    double getenergy(Eigen::VectorXd const & c, double ep, boost::multi_array<double, 2> const & h);

    //! A global function.
    /*!
        使用するGTOの数をユーザに入力させる
        \return 使用するGTOの数
    */
    std::int32_t input_nalpha();

    //! A global function.
    /*!
        GTOの肩の係数が格納された配列を生成する
        \param nalpha 使用するGTOの個数
        \return GTOの肩の係数が格納されたstd::vector
    */
    std::valarray<double> make_alpha(std::int32_t nalpha);

    //! A global function.
    /*!
        全ての要素が、引数で指定された値で埋められたnalpha次元ベクトルを生成する
        \param nalpha 使用するGTOの個数
        \param val 要素を埋める値
        \return 引数で指定された値で埋められたベクトル (Eigen::VectorXd)
    */
    Eigen::VectorXd make_c(std::int32_t nalpha, double val);

    //! A global function.
    /*!
        nalphaの数で、固有ベクトル、1電子積分および2電子積分からFock行列を生成する
        \param c 固有ベクトルC
        \param h 1電子積分hpq
        \param q 2電子積分Qprqs
        \return Fock行列 (Eigen::MatrixXd)
    */
    Eigen::MatrixXd make_fockmatrix(Eigen::VectorXd const & c, boost::multi_array<double, 2> const & h, boost::multi_array<double, 4> const & q);

    //! A global function.
    /*!
        1電子積分が格納された、nalpha×nalphaの2次元配列を生成する
        \param alpha GTOの肩の係数が格納されたstd::vector
        \return 1電子積分が格納された2次元配列 (boost::multi_array)
    */
    boost::multi_array<double, 2> make_oneelectroninteg(std::valarray<double> const & alpha);

    //! A global function.
    /*!
        nalpha次正方行列の重なり行列を生成する
        \param alpha GTOの肩の係数が格納されたstd::vector
        \return 重なり行列 (Eigen::MatrixXd)
    */
    Eigen::MatrixXd make_overlapmatrix(std::valarray<double> const & alpha);

    //! A global function.
    /*!
        2電子積分が格納されたnalpha×nalpha×nalpha×nalphaの4次元配列を生成する
        \param alpha GTOの肩の係数が格納されたstd::vector
        \return 2電子積分が格納された4次元配列 (boost::multi_array)
    */
    boost::multi_array<double, 4> make_twoelectroninteg(std::valarray<double> const & alpha);
}

int main()
{
    using namespace std::chrono;

    auto const start = system_clock::now();

    if (auto const res(do_scfloop()); res) {
        std::cout << boost::format("SCF計算が収束しました: energy = %.14f (Hartree)") % (*res) << std::endl;

        auto const end = system_clock::now();
        std::cout << boost::format("計算時間 = %.14f（秒）\n") % duration_cast< duration<double> >(end - start).count();

        return 0;
    }
    else {
        std::cerr << "SCF計算が収束しませんでした" << std::endl;

        return -1;
    }
}

namespace {
    std::optional<double> do_scfloop()
    {
        // 使用するGTOの数を入力
        //auto const nalpha(input_nalpha());
        int nalpha = 6;

        // GTOの肩の係数が格納された配列を生成
        auto alpha = make_alpha(nalpha);

        // 1電子積分が格納された2次元配列を生成
        auto const h(make_oneelectroninteg(alpha));

        // 2電子積分が格納された4次元配列を生成
        auto const q(make_twoelectroninteg(alpha));

        // 重なり行列を生成
        auto const s(make_overlapmatrix(alpha));

        // 全て0.0で初期化された固有ベクトルを生成
        auto c(make_c(nalpha, 0.0));

        // 新しく計算されたエネルギー
        auto enew = 0.0;

        // SCFループ
        for (auto iter = 1; iter < MAXITER; iter++) {
            // Fock行列を生成
            auto const f(make_fockmatrix(c, h, q));

            // 一般化固有値問題を解く
            Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(f, s);

            // E'を取得
            auto const ep = es.eigenvalues()[0];

            // 固有ベクトルを取得
            c = es.eigenvectors().col(0);

            // 前回のSCF計算のエネルギーを保管
            auto const eold = enew;

            // 今回のSCF計算のエネルギーを計算する
            enew = getenergy(c, ep, h);

            std::cout << boost::format("Iteration # %2d: HF eigenvalue = %.14f, energy = %.14f\n") % iter % ep % enew;

            // SCF計算が収束したかどうか
            if (std::fabs(enew - eold) < SCFTHRESHOLD) {
                // 収束したのでそのエネルギーを返す
                return std::make_optional(enew);
            }
        }

        // SCF計算が収束しなかった
        return std::nullopt;
    }

    double getenergy(Eigen::VectorXd const & c, double ep, boost::multi_array<double, 2> const & h)
    {
        auto const nalpha = static_cast<std::int32_t>(c.size());
        auto e = ep;
        for (auto p = 0; p < nalpha; p++) {
            for (auto q = 0; q < nalpha; q++) {
                // E = E' + Cp * Cq * hpq
                e += c[p] * c[q] * h[p][q];
            }
        }

        return e;
    }

    std::int32_t input_nalpha()
    {
        std::int32_t nalpha;

        while (true) {
            std::cout << "使用するGTOの個数を入力してください (3, 4 or 6): ";
            std::cin >> nalpha;

            if (!std::cin.fail() && (nalpha == 3 || nalpha == 4 || nalpha == 6)) {
                break;
            }

            std::cin.clear();
            std::cin.ignore(MAXBUFSIZE, '\n');
        }

        return nalpha;
    }

    std::valarray<double> make_alpha(std::int32_t nalpha)
    {
        switch (nalpha) {
        case 3:
            return { 0.31364978999999998, 1.1589229999999999, 6.3624213899999997 };

        case 4:
            return { 0.297104, 1.236745, 5.749982, 38.2166777 };

        case 6:
            return { 0.18595935599999999, 0.45151632200000003, 1.1627151630000001, 3.384639924, 12.09819836, 65.984568240000002 };

        default:
            BOOST_ASSERT(!"switch文のdefaultに来てしまった！");
            return std::valarray<double>();
        }
    }

    Eigen::VectorXd make_c(std::int32_t nalpha, double val)
    {
        Eigen::VectorXd c(nalpha);

        // 固有ベクトルCの要素を全てvalで初期化
        for (auto i = 0; i < nalpha; i++) {
            c[i] = val;
        }

        return c;
    }

    Eigen::MatrixXd make_fockmatrix(Eigen::VectorXd const & c, boost::multi_array<double, 2> const & h, boost::multi_array<double, 4> const & q)
    {
        auto const nalpha = static_cast<std::int32_t>(c.size());
        Eigen::MatrixXd f = Eigen::MatrixXd::Zero(nalpha, nalpha);

        for (auto p = 0; p < nalpha; p++) {
            for (auto qi = 0; qi < nalpha; qi++) {
                // Fpq = hpq + ΣCr * Cs * Qprqs
                f(p, qi) = h[p][qi];

                for (auto r = 0; r < nalpha; r++) {
                    for (auto s = 0; s < nalpha; s++) {
                        f(p, qi) += c[r] * c[s] * q[p][r][qi][s];
                    }
                }
            }
        }

        return f;
    }

    boost::multi_array<double, 2> make_oneelectroninteg(std::valarray<double> const & alpha)
    {
        using namespace boost::math::constants;

        auto const nalpha = static_cast<std::int32_t>(alpha.size());
        boost::multi_array<double, 2> h(boost::extents[nalpha][nalpha]);

        for (auto p = 0; p < nalpha; p++) {
            for (auto q = 0; q < nalpha; q++) {
                // αp + αq
                auto const appaq = alpha[p] + alpha[q];

                // hpq = 3αpαqπ^1.5 / (αp + αq)^2.5 - 4π / (αp + αq)
                h[p][q] = 3.0 * alpha[p] * alpha[q] * std::pow((pi<double>() / appaq), 1.5) / appaq -
                          4.0 * pi<double>() / appaq;
            }
        }

        return h;
    }

    Eigen::MatrixXd make_overlapmatrix(std::valarray<double> const & alpha)
    {
        using namespace boost::math::constants;

        auto const nalpha = static_cast<std::int32_t>(alpha.size());
        Eigen::MatrixXd s = Eigen::MatrixXd::Zero(nalpha, nalpha);

        for (auto p = 0; p < nalpha; p++) {
            for (auto q = 0; q < nalpha; q++) {
                // Spq = (π / (αp + αq))^1.5
                s(p, q) = std::pow((pi<double>() / (alpha[p] + alpha[q])), 1.5);
            }
        }

        return s;
    }

    boost::multi_array<double, 4> make_twoelectroninteg(std::valarray<double> const & alpha)
    {
        using namespace boost::math::constants;

        auto const nalpha = static_cast<std::int32_t>(alpha.size());
        boost::multi_array<double, 4> q(boost::extents[nalpha][nalpha][nalpha][nalpha]);

        for (auto p = 0; p < nalpha; p++) {
            for (auto qi = 0; qi < nalpha; qi++) {
                for (auto r = 0; r < nalpha; r++) {
                    for (auto s = 0; s < nalpha; s++) {
                        // Qprqs = 2π^2.5 / [(αp + αq)(αr + αs)√(αp + αq + αr + αs)]
                        q[p][r][qi][s] = 2.0 * std::pow(pi<double>(), 2.5) /
                            ((alpha[p] + alpha[qi]) * (alpha[r] + alpha[s]) *
                            std::sqrt(alpha[p] + alpha[qi] + alpha[r] + alpha[s]));
                    }
                }
            }
        }

        return q;
    }
}


#pragma once
#include <Eigen/Dense>
#include <unsupported/Eigen/KroneckerProduct>

#include "yavque/utils.hpp"
Eigen::VectorXcd product_state(uint32_t n, const Eigen::VectorXcd& s)
{
	Eigen::VectorXcd res(1);
	res(0) = 1.0;
	for(uint32_t i = 0; i < n; i++)
	{
		res = Eigen::kroneckerProduct(res, s).eval();
	}
	return res;
}

template<typename T>
Eigen::VectorXcd
apply_kronecker(uint32_t N, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& m,
                const Eigen::VectorXcd& vec)
{
	Eigen::VectorXcd res = vec;
	for(uint32_t k = 0; k < N; ++k)
	{
		res = yavque::apply_single_qubit(res, m, k);
	}
	return res;
}

template<typename RandomEngine>
std::pair<uint32_t, uint32_t> random_connection(const int N, RandomEngine& re)
{
	std::uniform_int_distribution<uint32_t> uid1(0, N - 1);
	std::uniform_int_distribution<uint32_t> uid2(0, N - 2);

	auto r1 = uid1(re);
	auto r2 = uid2(re);

	if(r2 < r1)
		return std::make_pair(r1, r2);
	else
		return std::make_pair(r1, r2 + 1);
}

template<typename RandomEngine>
Eigen::MatrixXcd random_unitary(uint32_t dim, RandomEngine&& re)
{
	constexpr yavque::cx_double I(0.0, 1.0);
	std::normal_distribution<double> ndist;

	Eigen::MatrixXcd m(dim, dim);
	for(uint32_t i = 0; i < dim; ++i)
	{
		for(uint32_t j = 0; j < dim; ++j)
		{
			m(i, j) = ndist(re) + I * ndist(re);
		}
	}
	Eigen::HouseholderQR<Eigen::MatrixXcd> qr(m);
	return qr.householderQ();
}

template<typename RandomEngine>
Eigen::VectorXcd random_vector(uint32_t dim, RandomEngine&& re)
{
	constexpr yavque::cx_double I(0.0, 1.0);
	std::normal_distribution<double> ndist;

	Eigen::VectorXcd res(dim);
	for(uint32_t k = 0; k < dim; ++k)
	{
		res(k) = ndist(re) + I * ndist(re);
	}
	res.normalize();
	return res;
}


#include <iostream>
#include <stdlib.h>
#include <vector>
#include <math.h>
#include <fstream>
#include <boost/qvm/vec.hpp>
#include <boost/qvm/vec_operations.hpp>
#include <boost/qvm/vec_access.hpp>
#include <boost/qvm/mat.hpp>
#include "Body.h"
#include "script_reader.h"

const double G = 100.0;
const double S = 0.05;

boost::qvm::vec<double, 3> acceleration_gravity(Body& b1, Body& b2)
{
    boost::qvm::vec<double, 3> r = b2.r - b1.r;
    boost::qvm::vec<double, 3> numerator = G * b2.m * r;
    double denominator = pow((pow(boost::qvm::mag(r), 2) + pow(S, 2)), 1.5);
    return numerator / denominator;
}

boost::qvm::vec<double, 3> net_acceleration(Body& b, std::vector<Body>& system)
{
    boost::qvm::vec<double, 3> a = { 0, 0, 0 };
    int i = 0;

    for (std::vector<Body>::const_iterator it = std::begin(system);
        it != std::end(system); it++, i++)
    {
        if (b.r != system[i].r)
        {
            a += acceleration_gravity(b, system[i]);
        }
    }

    return a;
}

// velocity verlet integrator - step forward once by dt
void verlet_step(std::vector<Body>& system, const double& dt)
{
    int i = 0;

    for (std::vector<Body>::const_iterator it = std::begin(system);
        it != std::end(system); it++, i++)
    {
        boost::qvm::vec<double, 3> a1 = net_acceleration(system[i], system);
        system[i].r += system[i].v * dt + 0.5 * a1 * pow(dt, 2);
        boost::qvm::vec<double, 3> a2 = net_acceleration(system[i], system);
        system[i].v += 0.5 * (a1 + a2) * dt;
    }
}

void integrate(initial_state& state, char* output_file_path)
{
    double t_current = 0;
    double t_save_current = 0;
    std::ofstream output_file(output_file_path);
    output_file << "t\tid\tm\trx\try\trz\n";
    output_file << std::scientific;
    int i = 0;

    while (t_current < state.t)
    {
        verlet_step(state.system, state.dt);

        if (t_save_current >= state.save_interval)
        {
            // save to file with format time \t id \t mass \t rx \t ry \t rz
            for (std::vector<Body>::const_iterator it = std::begin(state.system);
                it != std::end(state.system); it++, i++)
            {
                output_file << t_current << "\t"
                    << i + 1 << "\t"
                    << state.system[i].m << "\t"
                    << boost::qvm::A<0>(state.system[i].r) << "\t"
                    << boost::qvm::A<1>(state.system[i].r) << "\t"
                    << boost::qvm::A<2>(state.system[i].r) << "\n";
            }

            i = 0;
            t_save_current = 0;
        }
        else
        {
            t_save_current += state.dt;
        }

        t_current += state.dt;
    }
}

int main(int argc, char* argv[])
{
    if (argc == 3)
    {
        char* input_file_path = argv[1];
        char* output_file_path = argv[2];
        initial_state input_state = load_input_data(input_file_path);
        integrate(input_state, output_file_path);
    }
    else
    {
        std::cout << "error: takes two args: arg1 = input_file_path"
            << " arg2 = output_file_path" << std::endl;
        exit(EXIT_FAILURE);
    }

    return 0;
}


/*
This file is part of Bohrium and Copyright (c) 2012 the Bohrium team:
http://bohrium.bitbucket.org

Bohrium is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3
of the License, or (at your option) any later version.

Bohrium 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
GNU General Public License for more details.

You should have received a copy of the
GNU Lesser General Public License along with bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <armadillo>
#include <bp_util.h>

using namespace std;
using namespace arma;

template <typename T>
Col<T> cnd(Col<T> x)
{
    size_t samples = x.n_elem;
    Col<T> l(samples), k(samples), w(samples);
    T a1 = 0.31938153,
      a2 =-0.356563782,
      a3 = 1.781477937,
      a4 =-1.821255978,
      a5 = 1.330274429,
      pp = 2.5066282746310002; // sqrt(2.0*PI)

    l = abs(x);
    k = 1.0 / (1.0 + 0.2316419 * l);

    w = 1.0 - 1.0 / pp * exp(-1.0*l%l/2.0) % \
        (a1*k + \
         a2*(pow(k,(T)2)) + \
         a3*(pow(k,(T)3)) + \
         a4*(pow(k,(T)4)) + \
         a5*(pow(k,(T)5)));

    uvec mask = x < 0.0;

    return w % (-mask) + (1.0-w) % mask;
}

template <typename T>
T* pricing(size_t samples, size_t iterations, char flag, T x, T d_t, T r, T v)
{
    T* p    = (T*)malloc(sizeof(T)*samples);    // Intermediate results
    T t     = d_t;                              // Initial delta

    Col<T> d1(samples), d2(samples), res(samples);
    Col<T> s = randu<Col<T> >(samples)*4.0 +58.0;      // Model between 58-62

    for(size_t i=0; i<iterations; i++) {
        d1 = (log(s/x) + (r+v*v/2.0)*t) / (v*sqrt(t));
        d2 = d1-v*sqrt(t);
        if (flag == 'c') {
            res = s % cnd<T>(d1) -x * exp(-r*t) * cnd<T>(d2);
        } else {
            res = x * exp(-r*t) * cnd<T>(-1.0*d2) - s*cnd<T>(-1.0*d1);
        }

        t += d_t;                               // Increment delta
        p[i] = sum(res) / (T)samples;           // Result from timestep
    }

    return p;
}

int main(int argc, char* argv[])
{
    bp_util_type bp = bp_util_create(argc, argv, 2);
    if (bp.args.has_error) {
        return 1;
    }
    const size_t samples    = bp.args.sizes[0];
    const size_t iterations = bp.args.sizes[1];

    bp.timer_start();
    double* prices = pricing(
        samples, iterations,
        'c', 65.0, 1.0 / 365.0,
        0.08, 0.3
    );
    bp.timer_stop();

    bp.print("black_scholes(cpp11_armadillo)");
    if (bp.args.verbose) {
        cout << ", \"output\": [";
        for(size_t i=0; i<iterations; i++) {
            cout << prices[i];
            if (iterations-1!=i) {
                cout << ", ";
            }
        }
        cout << "]" << endl;
    }

    free(prices);
    return 0;
}


/*
 * =====================================================================================
 *
 *       Filename:  optimal_transport.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  11/04/19 10:55:59
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  YOUR NAME (),
 *   Organization:
 *
 * =====================================================================================
 */
#include <stdlib.h>
#include <iostream>
#include <Eigen/Dense>
#include "optimal_transport.h"

using namespace Eigen;
typedef Map<VectorXd> MapT;

// TODO how to do it passable via cffi?
// const double lambda = 1;
// const double epsilon = 0.1;
// const double tol = 1e-05;
// const double threshold = 1e+02;
// const double max_iter = 1000;

VectorXd proxdiv_TV(VectorXd s, VectorXd u, VectorXd p, double lambda,
                    double epsilon) {
    ArrayXd e_lam = ((lambda - u.array()) / epsilon).exp();
    ArrayXd e_minus_lam = (-(lambda + u.array()) / epsilon).exp();
    return e_lam.cwiseMin(e_minus_lam.cwiseMax(p.array() / s.array())).matrix();
}

VectorXd proxdiv_i(VectorXd s, VectorXd u, VectorXd p, double lambda,
                   double epsilon) {
    return (p.array() / s.array()).matrix();
}

MatrixXd calc_distance_matrix(VectorXd mzs1, VectorXd mzs2) {
    return (mzs1 * VectorXd::Ones(mzs2.rows()).transpose() -
                VectorXd::Ones(mzs1.rows()) * mzs2.transpose()).cwiseAbs();
}

MatrixXd calc_transport_plan(VectorXd ints1, VectorXd ints2, MatrixXd dists,
                             double lambda, double epsilon, double tol,
                             double threshold, double max_iter) {
    int n1 = ints1.rows();
    int n2 = ints2.rows();
    MatrixXd K_0 = (-dists / epsilon).array().exp().matrix();
    MatrixXd K = K_0;

    VectorXd b = VectorXd::Ones(n2);
    VectorXd a = VectorXd::Zero(n1);
    VectorXd u = VectorXd::Zero(n1);
    VectorXd v = VectorXd::Zero(n2);
	VectorXd a_new, b_new;
    double coverging_val;

    int tick = 0;
    bool coverged = false;

    do {
        a_new = proxdiv_TV(K * b, u, ints1, lambda, epsilon); // * is matrix-matrix
        b_new = proxdiv_TV(K.transpose() * a_new, v, ints2, lambda, epsilon);

        if ((((a_new.array().log().abs() - threshold) > 0).any()) or
            (((b_new.array().log().abs() - threshold) > 0).any())) {
            // Stabilizing
            u += epsilon * a_new.array().log().matrix();
            v += epsilon * b_new.array().log().matrix();
            // below is exp((u_i + v_j - dists_{ij}) / eps) forall i,j
            K = (u / epsilon).array().exp().matrix().asDiagonal() * K_0 *
                (v / epsilon).array().exp().matrix().asDiagonal();
        }

        coverging_val = std::max((a_new - a).maxCoeff(),
                                 (b_new - b).maxCoeff());
        tick++;
        coverged = coverging_val < tol or tick >= max_iter;
        a = a_new;
        b = b_new;
    } while (not coverged);
    // below is (a_i * K_{ij} * b_j)_{ij}
    return a.asDiagonal() * K * b.asDiagonal();
}

double calc_distance_from_plan(MatrixXd transport_plan, MatrixXd distances,
                               VectorXd ints1, VectorXd ints2, double lambda) {
    double transport = (transport_plan.array() * distances.array()).sum();
    // ^^^ coeff-wise multiplication ^^^

    double trash = 0;
    trash += lambda * ((transport_plan.rowwise().sum() - ints1).cwiseAbs().sum());
    trash += lambda * ((transport_plan.colwise().sum().transpose() - ints2).cwiseAbs().sum());
    // ^^ colwise sum is row, not column
    return transport + trash;
}

double calc_distance_cpp(double* mzs1, double* ints1, int len1, double* mzs2,
                         double* ints2, int len2, double lambda,
                         double epsilon, double tol, double threshold,
                         double max_iter) {
    /*  TODO description
     *
     *  ints1, and ints2 should be normalized */
    /*
     * TODO sharing memory causes segfault, so maybe copying to matrix is
     * better?
     */

    MapT mzs1_map(mzs1, len1);
    MapT mzs2_map(mzs2, len2);
    MapT ints1_map(ints1, len1);
    MapT ints2_map(ints2, len2);
    MatrixXd dists = calc_distance_matrix(mzs1_map, mzs2_map);
    MatrixXd transport_plan = calc_transport_plan(ints1_map, ints2_map, dists,
            lambda, epsilon, tol, threshold, max_iter);
    return calc_distance_from_plan(transport_plan, dists, ints1_map, ints2_map,
                                   lambda);
}

double calc_distance_c(double* mzs1, double* ints1, int len1, double* mzs2,
                       double* ints2, int len2, double lambda, double epsilon,
                       double tol, double threshold, double max_iter) {
    return calc_distance_cpp(mzs1, ints1, len1, mzs2, ints2, len2, lambda,
                             epsilon, tol, threshold, max_iter);
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file
 * \author   Collin Johnson
 *
 * Definition of independent_t_test.
 */

#include "math/t_test.h"
#include <boost/math/distributions/students_t.hpp>

namespace vulcan
{
namespace math
{

t_test_results_t independent_t_test(const t_test_sample_t& sampleA, const t_test_sample_t& sampleB, double confidence)
{
    // Code closely related to Boost.Math example at:
    // http://www.boost.org/doc/libs/1_65_1/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/two_sample_students_t.html

    using namespace boost::math;

    assert(sampleA.numSamples > 1);
    assert(sampleB.numSamples > 1);
    assert(sampleA.variance > 0.0);
    assert(sampleB.variance > 0.0);

    int dofA = sampleA.numSamples - 1;
    int dofB = sampleB.numSamples - 1;
    int dof = dofA + dofB;

    double pooledVariance = std::sqrt(((dofA * sampleA.variance) + (dofB * sampleB.variance)) / dof);
    double tDenom = pooledVariance * std::sqrt((1.0 / sampleA.numSamples) + (1.0 / sampleB.numSamples));

    t_test_results_t results;
    results.tValue = (sampleA.mean - sampleB.mean) / tDenom;
    results.confidence = confidence;

    students_t dist(dof);

    results.pValueDifferent = cdf(complement(dist, std::abs(results.tValue)));
    results.areDifferent = results.pValueDifferent < (confidence / 2.0);

    results.pValueGreater = cdf(complement(dist, results.tValue));
    results.isGreater = results.pValueGreater < confidence;

    results.pValueLess = cdf(dist, results.tValue);
    results.isLess = results.pValueLess < confidence;

    return results;
}

}   // namespace math
}   // namespace vulcan


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_STIRLING_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_STIRLING_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
 /*!
    @ingroup group-euler
    This function object computes stirling formula for the gamma function

    @par Semantic:
    For every parameter of floating type @c T , the following code:
    @code
    T r = stirling(x);
    @endcode
    computes  \f$\sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.
    The formula implementation is usable for x between 33 and 172 to approximate \f$\Gamma(x)\f$.

    @see gamma, gammaln

    @param v0 value of a floating-point
    @return The aproximation of \f$\Gamma(v_0)\f$ using Striling's formula.
  **/
  Value stirling(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/stirling.hpp>
#include <boost/simd/function/simd/stirling.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// skewness.hpp
//
//  Copyright 2006 Olivier Gygi, Daniel Egloff. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_SKEWNESS_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_SKEWNESS_HPP_EAN_28_10_2005

#include <limits>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/mean.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // skewness_impl
    /**
        @brief Skewness estimation

        The skewness of a sample distribution is defined as the ratio of the 3rd central moment and the \f$ 3/2 \f$-th power
        of the 2nd central moment (the variance) of the sampless 3. The skewness can also be expressed by the simple moments:

        \f[
            \hat{g}_1 =
                \frac
                {\widehat{m}_n^{(3)}-3\widehat{m}_n^{(2)}\hat{\mu}_n+2\hat{\mu}_n^3}
                {\left(\widehat{m}_n^{(2)} - \hat{\mu}_n^{2}\right)^{3/2}}
        \f]

        where \f$ \widehat{m}_n^{(i)} \f$ are the \f$ i \f$-th moment and \f$ \hat{\mu}_n \f$ the mean (first moment) of the
        \f$ n \f$ samples.
    */
    template<typename Sample>
    struct skewness_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, Sample>::result_type result_type;

        skewness_impl(dont_care)
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::average(
                        accumulators::moment<3>(args)
                        - 3. * accumulators::moment<2>(args) * mean(args)
                        + 2. * mean(args) * mean(args) * mean(args)
                      , ( accumulators::moment<2>(args) - mean(args) * mean(args) )
                        * std::sqrt( accumulators::moment<2>(args) - mean(args) * mean(args) )
                   );
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::skewness
//
namespace tag
{
    struct skewness
      : depends_on<mean, moment<2>, moment<3> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::skewness_impl<mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::skewness
//
namespace extract
{
    extractor<tag::skewness> const skewness = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(skewness)
}

using extract::skewness;

// So that skewness can be automatically substituted with
// weighted_skewness when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::skewness>
{
    typedef tag::weighted_skewness type;
};

template<>
struct feature_of<tag::weighted_skewness>
  : feature_of<tag::skewness>
{
};

}} // namespace boost::accumulators

#endif


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Random.h>

#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>

#include <CGAL/boost/graph/graph_traits_PolyMesh_ArrayKernelT.h>
#include <CGAL/boost/graph/properties_PolyMesh_ArrayKernelT.h>

#include <CGAL/boost/graph/iterator.h>

#include <CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path_traits.h>
#include <CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path.h>

#include <boost/lexical_cast.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef OpenMesh::PolyMesh_ArrayKernelT<> Triangle_mesh;

typedef boost::graph_traits<Triangle_mesh> Graph_traits;
typedef Graph_traits::vertex_descriptor vertex_descriptor;
typedef Graph_traits::vertex_iterator vertex_iterator;
typedef Graph_traits::face_descriptor face_descriptor;
typedef Graph_traits::face_iterator face_iterator;

typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Triangle_mesh> Traits;
typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;

int main(int argc, char** argv)
{
  // read the input surface mesh
  Triangle_mesh tmesh;
  OpenMesh::IO::read_mesh(tmesh, (argc>1)?argv[1]:"data/elephant.off");

  // pick up a random face
  const unsigned int randSeed = argc > 2 ? boost::lexical_cast<unsigned int>(argv[2]) : 7915421;
  CGAL::Random rand(randSeed);
  const int target_face_index = rand.get_int(0, static_cast<int>(num_faces(tmesh)));
  face_iterator face_it = faces(tmesh).first;
  std::advance(face_it,target_face_index);
  // ... and define a barycentric coordinates inside the face
  Traits::Barycentric_coordinates face_location = {{0.25, 0.5, 0.25}};

  // construct a shortest path query object and add a source point
  Surface_mesh_shortest_path shortest_paths(tmesh);
  shortest_paths.add_source_point(*face_it, face_location);

  // For all vertices in the tmesh, compute the points of
  // the shortest path to the source point and write them
  // into a file readable using the CGAL Tmesh demo
  std::ofstream output("shortest_paths_OpenMesh.cgal");
  vertex_iterator vit, vit_end;
  for ( boost::tie(vit, vit_end) = vertices(tmesh);
        vit != vit_end; ++vit)
  {
    std::vector<Traits::Point_3> points;
    shortest_paths.shortest_path_points_to_source_points(*vit, std::back_inserter(points));

    // print the points
    output << points.size() << " ";
    for (std::size_t i = 0; i < points.size(); ++i)
      output << " " << points[i];
    output << std::endl;
  }

  return 0;
}


#include "global.h"
#include "distr.h"
#include "utils.h"

#include <omp.h>
#include <armadillo>
#include <cmath>
#include <limits>
#include <vector>
#include <random>
#include <boost/math/special_functions/erf.hpp> // can I do this?
#include <iostream>

//defined in global.h
extern std::vector<std::mt19937_64> rng;

namespace Distributions{

	double randU01()
	{
		std::uniform_real_distribution<> distr(0, std::nextafter(1, std::numeric_limits<double>::max())); // init U(0,1)
		double res = distr(rng[omp_get_thread_num()]);
		return res;
	}

	double randLogU01()
	{
		std::uniform_real_distribution<> distr(0, std::nextafter(1, std::numeric_limits<double>::max())); // init U(0,1)
		double res = log(distr(rng[omp_get_thread_num()]));
		return res;
	}

	int randIntUniform(const int a,const int b)
	{
		std::uniform_int_distribution<> distr(a, b); // init the discrete uniform
		double res = distr(rng[omp_get_thread_num()]);
		return res;
	}

	arma::ivec randIntUniform(const unsigned int n, const int a,const int b)
	{
		arma::ivec res(n);
		std::uniform_int_distribution<> distr(a, b); // init the discrete uniform
		for(unsigned int i=0; i<n; ++i)
		{
			res(i) = distr(rng[omp_get_thread_num()]);
		}
		return res;
	}

	double randExponential(const double lambda)
	{
		std::exponential_distribution<> distr(lambda);
		double res = distr(rng[omp_get_thread_num()]);
		return res;
	}

	arma::vec randExponential(const unsigned int n, const double lambda)
	{
		arma::vec res(n);
		std::exponential_distribution<> distr(lambda);
		for(unsigned int i=0; i<n; ++i)
		{
			res(i) = distr(rng[omp_get_thread_num()]);
		}
		return res;
	}

	unsigned int randBinomial(const unsigned int n, const double p) // slow but safe (CARE, n here is the binomial parameters, return value is always ONE integer)
	{
		std::binomial_distribution<> d(n, p);
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	arma::uvec randMultinomial(unsigned int n, const arma::vec prob)
	{

	  unsigned int K = prob.n_elem;
	  arma::uvec rN = arma::zeros<arma::uvec>(K);
	  double p_tot = sum(prob);
	  double pp;

	  for(unsigned int k = 0 ; k < (K-1) ; ++k)
	  {
	    if(prob(k)>0) {
	    	pp = prob(k) / p_tot;
	    	rN(k) = ((pp < 1.) ? randBinomial(n,  pp) : n);
	    	n -= rN(k);
	    }else{
	    	rN(k) = 0;
	    }

	    if(n <= 0) /* we have all*/ return rN;
	    p_tot -= prob(k); /* i.e. = sum(prob[(k+1):K]) */
	  }
	  rN(K-1) = n - sum(rN);
	  return rN;

	}

	double randNormal(const double m=0., const double sigmaSquare=1.) // random normal interface, parameters mean and variance
	{
    	std::normal_distribution<> d(m,sqrt(sigmaSquare));
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	arma::vec randNormal(const unsigned int n, const double m=0., const double sigmaSquare=1.) // n-sample normal, parameters mean and variance
	{
    	arma::vec res(n);
    	std::normal_distribution<> d(m,sqrt(sigmaSquare));
    	for(unsigned int i=0; i<n; ++i)
		{
			res(i) = d(rng[omp_get_thread_num()]);
		}
		return res;
	}

	arma::vec randMvNormal(const arma::vec &m, const arma::mat &Sigma) // random normal interface to arma::randn
	{
		unsigned int d = m.n_elem;

		//check
		if(Sigma.n_rows != d || Sigma.n_cols != d )
		{
			std::cout << " Dimension not matching in the multivariate normal sampler" << std::flush;
			return 0;
		}

			arma::mat A;
			arma::vec eigval;
			arma::mat eigvec;
			arma::rowvec res;

			if( arma::chol(A,Sigma) )
			{
				res = randNormal(d).t() * A ;
			}
			else
			{
	// std::cout << Sigma << std::endl << std::endl;
	// std::cin >> d; d = m.n_elem;
				if( eig_sym(eigval, eigvec, Sigma) )
				{
					res = (eigvec * arma::diagmat(arma::sqrt(eigval)) * randNormal(d)).t();
				}else{
					std::cout << "randMvNorm failing because of singular Sigma matrix" << std::endl << std::flush;
					throw;
				}
			}

		return res.t() + m;
	}

	double randT(const double nu)
	{
    	std::student_t_distribution<double> d(nu);
		double res = d(rng[omp_get_thread_num()]);;
		return res;
	}

	arma::vec randT(const unsigned int n, const double nu)
	{
    	arma::vec res(n);
    	std::student_t_distribution<double> d(nu);
    	for(unsigned int i=0; i<n; ++i)
		{
			res(i) = d(rng[omp_get_thread_num()]);
		}
		return res;
	}

	arma::vec randMvT(const double &nu, const arma::vec &m, const arma::mat &Sigma)
	{
		unsigned int d = m.n_elem;

		//check
		if(Sigma.n_rows != d || Sigma.n_cols != d )
		{
			std::cout << " Dimension not matching in the multivariate t sampler" << std::flush;
			throw; // THROW EXCPTION
		}

		arma::rowvec res = randT(d,nu).t() * arma::chol(Sigma);

		return res.t() + m;
	}

	double randGamma(double shape, double scale)   // shape scale parametrisation
	{
		//check
		if(shape <= 0 || scale <= 0 )
		{
			std::cout << " Negative parameter in the gamma sampler " << std::flush;
			throw; // THROW EXCPTION
		}

		std::gamma_distribution<> d(shape,scale);
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	double randIGamma(double shape, double scale)
	{
		//check
		if(shape <= 0 || scale <= 0 )
		{
			std::cout << " Negative parameter in the gamma sampler " << std::flush;
			throw; // THROW EXCPTION
		}

		std::gamma_distribution<> d(shape,1./scale);
		double res =  ( 1./d(rng[omp_get_thread_num()]) );
		return res;
	}

	arma::mat randWishart(double df, const arma::mat& S)   // unsigned int df is obsolete, I see no reason to keep it
	{
		// Dimension of returned wishart
		unsigned int m = S.n_rows;

		// Z composition:
		// sqrt chisqs on diagonal (with different parameters, so no need to create the distribution object here)
		// random normals below diagonal
		std::normal_distribution<> normal01(0.,1.);
		// misc above diagonal
		arma::mat Z(m,m);

		// Fill the diagonal
		for(unsigned int i = 0; i < m; i++){
			Z(i,i) = sqrt( randGamma( (df-i)/2.,2. ) );    // (note it's df-1:m+1)
		}

		// Fill the lower matrix with random normals
		for(unsigned int j = 0; j < m; j++){
			for(unsigned int i = j+1; i < m; i++){
		  		Z(i,j) = normal01(rng[omp_get_thread_num()]);
			}
		}

		// Lower triangle * chol decomp
		arma::mat C = arma::trimatl(Z).t() * arma::chol(S);

		// Return random wishart
		return C.t()*C;
	}

	arma::mat randIWishart(double df, const arma::mat& S)
	{

  		return arma::inv_sympd( randWishart(df,S.i()) );   // return the inverse of the correspondent Wishart variate ... is this even fast enough?
	}

	arma::mat randMN(const arma::mat &M, const arma::mat &rowCov, const arma::mat &colCov)
	{
		arma::mat C = arma::chol( arma::kron(colCov,rowCov) );
		arma::mat z = randNormal( (unsigned int)(M.n_cols * M.n_rows) ).t() * C;
		z.reshape( arma::size(M) );
		return (z + M);
	}

	double randBeta(double a, double b)
	{
		double num = randGamma(a,1.);
		double den = randGamma(b,1.) + num;

		return num/den;
	}

	unsigned int randBernoulli(double pi)
	{
		std::bernoulli_distribution d(pi);
		double res = d(rng[omp_get_thread_num()]);
		return res;
	}

	double randTruncNorm(double m, double sd,double lower, double upper) // Naive, but it'll do for now -- notice now parameters are mean and standard deviation!
	{
		double ret = randNormal(m,sd);

		while( ret < lower || ret > upper)
			ret = randNormal(m,sd);

		return ret;
	}

	arma::uvec randSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::uvec& population, // population to draw from
	    unsigned int sampleSize        // size of each sample
	) // output, sample is a zero-offset indices to selected items, output is the subsampled populaiton.
	{
		arma::uvec samples(sampleSize);

	    int t = 0; // total input records dealt with
	    unsigned int m = 0; // number of items selected so far
	    double u;

	    while (m < sampleSize)
	    {
	        u = randU01(); // call a uniform(0,1) random number generator

	        if ( (populationSize - t)*u >= sampleSize - m )
	        {
	            t++;
	        }
	        else
	        {
	            samples(m) = t;
	            t++; m++;
	        }
	    }

	    return population(samples);
	}

	std::vector<unsigned int> randSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const std::vector<unsigned int>& population, // population to draw from
	    unsigned int sampleSize        // size of each sample
	) // output, sample is a zero-offset indices to selected items, output is the subsampled populaiton.
	{
		std::vector<unsigned int> samplesIndexes(sampleSize);

	    int t = 0; // total input records dealt with
	    unsigned int m = 0; // number of items selected so far
	    double u;

	    while (m < sampleSize)
	    {
	        u = randU01(); // call a uniform(0,1) random number generator

	        if ( (populationSize - t)*u >= sampleSize - m )
	        {
	            t++;
	        }
	        else
	        {
	            samplesIndexes[m] = t;
	            t++; m++;
	        }
	    }

		std::vector<unsigned int> res(sampleSize);
		m = 0;
		for( auto i : samplesIndexes )
		{
			res[m++] = population[i];
		}

	    return res;
	}

	// IMPLEMENTATION FROM Efraimidistr and Spirakis 2006
	// (probably efficient when n is close to N rather than in our case, but is there a better alternative?)
	//  even for n=1, we'd still need to compute the cumulative sum of all the weights if we want to use bisection, or what I use below which is O(N) anyway...
	arma::uvec randWeightedSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights,	   // probability for each element
	    unsigned int sampleSize,        // size of each sample
	    const arma::uvec& population // population to draw from
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{

	    arma::vec score = randExponential(populationSize,1.)/weights;
	    arma::uvec result = population( (arma::sort_index(weights,"ascend")) );

	    return result.subvec(0,sampleSize-1);
	}

	// overload with sampleSize equal to one
	arma::uword randWeightedSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights,	   // probability for each element
	    const arma::uvec& population // population to draw from
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
	    double u = randU01();
	    double tmp = weights(0);
	    int t = 0;

	    while(u > tmp)
	    {
	    	tmp += weights(++t);
	    }

	    return population( t );
	}

	// Versions that return indexes only
	arma::uvec randWeightedIndexSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights,	   // (log) probability for each element
	    unsigned int sampleSize         // size of each sample
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
		// note I can do everything in the log scale as the ordering won't change!
	    arma::vec score = randExponential(populationSize,1.) - weights;
	    arma::uvec result = arma::sort_index(score,"ascend");

	    return result.subvec(0,sampleSize-1);
	}

	// Overload with equal weights
	arma::uvec randWeightedIndexSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    unsigned int sampleSize         // size of each sample
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
		// note I can do everything in the log scale as the ordering won't change!
	    arma::vec score = randExponential(populationSize,1.);
	    arma::uvec result = arma::sort_index(score,"ascend");

	    return result.subvec(0,sampleSize-1);
	}

	// overload with sampleSize equal to one
	arma::uword randWeightedIndexSampleWithoutReplacement
	(
	    unsigned int populationSize,    // size of set sampling from
	    const arma::vec& weights     // probability for each element
	) // sample is a zero-offset indices to selected items, output is the subsampled population.
	{
		// note I can do everything in the log scale as the ordering won't change!

	    double u = randU01();
	    double tmp = weights(0);
	    unsigned int t = 0;

	    while(u > tmp)
	    {
	    	// tmp = Utils::logspace_add(tmp,logWeights(++t));
	    	tmp += weights(++t);
	    }

	    return t;
	}

	/// ################### NOW LOG PDFs

	// logPDF rand Weighted Indexes (need to implement the one for the original starting vector?)
	double logPDFWeightedIndexSampleWithoutReplacement(const arma::vec& weights, const arma::uvec& indexes)
	{
		// arma::vec logP_permutation = arma::zeros<arma::vec>((int)std::tgamma(indexes.n_elem+1));  //too big of a vector
		double logP_permutation = 0.; double tmp;

		std::vector<unsigned int> v = arma::conv_to<std::vector<unsigned int>>::from(arma::sort(indexes));
		// vector should be sorted at the beginning.

		arma::uvec current_permutation;
		arma::vec current_weights;
	 	unsigned int i = 0;

	    do {
	        current_permutation = arma::conv_to<arma::uvec>::from(v);
	        current_weights = weights;
			tmp = 0.;

			while( current_permutation.n_elem > 0 )
			{
			   tmp += log(current_weights(current_permutation(0)));
			   current_permutation.shed_row(0);
			   current_weights = current_weights/arma::sum(current_weights(current_permutation));   // this will gets array weights that do not sum to 1 in total, but will only use relevant elements
    		}

			++i;
			logP_permutation = Utils::logspace_add(logP_permutation,tmp);

	    } while (std::next_permutation(v.begin(), v.end()));

		return logP_permutation;
	}

	double logPDFIGamma(double x, double a, double b)
	{
		if( x < 0 || b < 0 || a < 0 )
			return -std::numeric_limits<double>::infinity();
		else
			return a*log(b) -std::lgamma(a) + (-a-1.)*log(x) -b/x;
	}

	double logPDFGamma(double x, double a, double b)
	{
		if( x < 0 || b < 0 || a < 0 )
			return -std::numeric_limits<double>::infinity();
		else
			return -a*log(b) -std::lgamma(a) + (a-1.)*log(x) -x/b;
	}

	double logPDFIWishart(const arma::mat& X, double nu, const arma::mat& Sigma)
	{
		unsigned int p = X.n_rows;
		double ret = -0.5*(double)p*nu*log(2) - lMvGamma(p,nu) - 0.5*arma::trace( Sigma * arma::inv_sympd(X) );
		double sign, tmp;
		arma::log_det(tmp, sign, X );
		ret += -0.5*( (double)p + nu + 1. )*tmp;

		arma::log_det(tmp, sign, Sigma );
		ret += +0.5*nu*tmp;

		return ret;
	}

	double logPDFMN(const arma::mat& X, const arma::mat& rowCov, const arma::mat colCov)
	{
		unsigned int n = X.n_rows;
		unsigned int m = X.n_cols;

		double ret = -0.5*arma::trace( arma::inv_sympd(colCov) * X.t() * arma::inv_sympd(rowCov) * X ) -
					(double)n*(double)m*0.5*log(2*M_PI);
		double sign, tmp;
		arma::log_det(tmp, sign, colCov );
		ret += -0.5*(double)n*tmp;

		arma::log_det(tmp, sign, rowCov );
		ret += -0.5*(double)m*tmp;

		return ret;
	}

 	double logPDFNormal(const double& x, const double& m,const  double& sigmaSquare)
	{

		return -0.5*log(2*M_PI) -0.5*log(sigmaSquare) -(0.5/sigmaSquare)*arma::as_scalar( pow(x-m,2) );

	}

	double logPDFNormal(const arma::vec& x, const  arma::mat& Sigma)  // zeroMean
	{
		unsigned int k = Sigma.n_cols;

		double sign, tmp;
		arma::log_det(tmp, sign, Sigma ); //sign is not importantas det SHOULD be > 0 as for positive definiteness!

		return -0.5*(double)k*log(2*M_PI) -0.5*tmp -0.5* arma::as_scalar( (x).t() * arma::inv_sympd(Sigma) * (x) );

	}

	double logPDFNormal(const arma::vec& x, const arma::vec& m,const  arma::mat& Sigma)
	{
		unsigned int k = Sigma.n_cols;

		double sign, tmp;
		arma::log_det(tmp, sign, Sigma ); //sign is not importantas det SHOULD be > 0 as for positive definiteness!

		return -0.5*(double)k*log(2*M_PI) -0.5*tmp -0.5* arma::as_scalar( (x-m).t() * arma::inv_sympd(Sigma) * (x-m) );

	}

	double logPDFNormal(const arma::vec& x, const arma::vec& m,const  double& Sigma)
	{

		//this is more a lok likelihood here, since the input vector is indep realisations with same sigma and (possibly) different means
		unsigned int n = x.n_elem;

		return -0.5*(double)n*log(2*M_PI) -0.5*n*log(Sigma) -0.5/Sigma * arma::as_scalar( (x-m).t() * (x-m) );

	}

	double logPDFNormal(arma::vec& x, arma::vec& m, const arma::mat& rowCov ,const arma::mat& colCov)   // vectorised version of a matrix normal
	{
		unsigned int k = rowCov.n_rows;
		unsigned int d = colCov.n_rows;

		double logP = -0.5*(double)k*log(2*M_PI) - 0.5 * arma::as_scalar( ( (x-m).t() * arma::inv_sympd( arma::kron( colCov , rowCov ) ) * (x-m) ) );

		double sign, tmp;
		arma::log_det(tmp, sign, rowCov );
		logP += -0.5*(double)d*tmp;

		arma::log_det(tmp, sign, colCov );
		logP += -0.5*(double)k*tmp;

		return logP;
	}

	double lBeta(double a,double b){    //log beta function
		return std::lgamma(a) + std::lgamma(b) - std::lgamma(a+b);
	}

	double logPDFBeta(double x, double a, double b)
	{
		if( x <= 0. || x >= 1. )
			return -std::numeric_limits<double>::infinity();
		else
			return -lBeta(a,b) + (a-1)*log(x) + (b-1)*log(1-x);
	}

	double logPDFBernoulli(unsigned int x, double pi)
	{
		if( x > 1 ) // remember x is UNSIGNED int here
			return -std::numeric_limits<double>::infinity();
		else
			return x*log(pi) + (1-x)*log(1.-pi);
	}

	double CDFNormal(double x, double m, double sd)
	{
		return 0.5 * std::erfc(-((x-m)/sd) * M_SQRT1_2); // ... right?
	}

	double invCDFNormal(double x, double m, double sd)
	{
		return sqrt(2.) * boost::math::erf_inv(2.*((x-m)/sd)-1.);; // ... right?
	}

	double logPDFTruncNorm(double x, double m, double sd, double lower, double upper)
	{
		double truncNormConst = log( CDFNormal(upper,m,sd) - CDFNormal(lower,m,sd) );
		return -log(sqrt(2*M_PI)) -log(sd) -0.5*(x-m)*(x-m)/(sd*sd) - truncNormConst;
	}

	double lMvGamma(unsigned int n, double a)					// Multivariate GAMMA FUNCTION! NOT THE PDF/CDF
	{
		double lmvG = 0;
		for(unsigned int j=0; j<n ; ++j)
		{
			lmvG += std::lgamma( a + 0.5*(1.-(double)j+1.) );   //last +1 cause indexes start from 0
		}

		return (n*(n-1.)*0.25)*log(M_PI) + lmvG;
	}

}


//
// fibonacci.cpp
//
// Created by massimo on 3/16/18.
//

#include "fibonacci.h"

#include <climits>
#include <iostream>
#include <boost/program_options.hpp>
////////////////////////////////////////////////////////////////////////////////
namespace po      = boost::program_options;
namespace postyle = boost::program_options::command_line_style;
////////////////////////////////////////////////////////////////////////////////

static
std::pair<bigint::bigint, bigint::bigint>
fast_fib_even(ui64 n)
{
  if ( 0 == n )
  {
    return std::make_pair(1, 0);
  }
  else if ( 2 == n )
  {
    return std::make_pair(1, 1);
  }
  else if ( 4 == n )
  {
    return std::make_pair(2, 3);
  }
  auto p = fast_fib((n >> 1) - 1);
  auto c = p.first + p.second;
  auto d = p.second + c;

  return std::make_pair(p.second * d + p.first * c, c * (d + p.second));
}

static
std::pair<bigint::bigint, bigint::bigint>
fast_fib(ui64 n)
{
 if (isOdd(n))
 {
   auto p = fast_fib_even(n - 1);
   return std::make_pair(p.second, p.first + p.second);
 }
 else
 {
   return fast_fib_even(n);
 }
}

static ui64 a;
static ui64 b;
static ui64 c;
static ui64 d;

static
void
fast_fib(ui64 n, ui64 ans[])
{
  if (0 == n)
  {
    ans[0] = 0;  // F(0)
    ans[1] = 1;  // F(1)
    return;
  }

  fast_fib((n >> 1), ans);

  a = ans[0];  // F(n)
  b = ans[1];  // F(n+1)
  c = 2 * b - a;

  c = (a * c);          // F(2n)
  d = (a * a + b * b);  // F(2n + 1)
  if (0 == n % 2)
  {
    ans[0] = c;
    ans[1] = d;
  }
  else
  {
    ans[0] = d;
    ans[1] = c + d;
  }
}

static
ui64
fastFib(ui64 n)
{
  if (n < 94)
  {
    ui64 ans[2] = {0};

    fast_fib(n, ans);

    return ans[0];
  }
  return 0;
}

static
bigint::bigint
fastFib(ui64 n, bool flag)
{
  if (n >= 94)
  {
    return fast_fib(n).second;
  }
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
int
main(const int argc, const char** argv)
{
  po::options_description desc("Options");

  desc.add_options()
          ("n,n",    po::value<ui64>()->implicit_value(1)->default_value(1), "the fib number we want to compute")
          ("test,t", po::value<bool>()->implicit_value(true)->default_value(false), "test fib algorithms; no result is printed or saved on file")
          ("help,h",   "Print this help message")
          ;

  po::variables_map vm;

  try
  {
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);

    po::notify(vm);

    if ( vm.count("help") )
    {
      std::clog << "[" << __func__ << "] "
                << "USAGE: "
                << argv[0]
                << " options\n"
                << desc
                << '\n';
      return 0;
    }
  }
  catch (const std::exception &e)
  {
    std::cerr << "[" << __func__ << "] "
              << "Error parsing command line: '"
              << e.what()
              << "'"
              << '\n';

    std::cerr << desc << '\n';

    return -1;
  }

  const ui64 N = vm["n"].as<ui64>();
  const bool testAlgo = vm["test"].as<bool>();
////////////////////////////////////////////////////////////////////////////////
  if ( N < 94 )
  {
    ui64 fN = fastFib(N);
    std::string sfN = std::to_string(fN);
    std::string fileName{"fib-" + std::to_string(N) + ".txt"};
    utilities::cfile_helper::cfile_helper fs = utilities::cfile_helper::cfile_helper(fileName,
                                                                                utilities::cfile_helper::cfile_helper::fstream_direction::fs_OUTPUT);
    fs.get_fstream() << sfN;

    std::cout << "[" << __func__ << "] "
              << "fib("
              << N
              << ") = "
              << sfN
              << "\nof length "
              << sfN.size()
              << " digits written to file "
              << fileName
              << std::endl;
  }
  else  // when N >= 94
  {
//    [[maybe_unused]]
//    auto
//    fib = [&N]() {
//      bigint::bigint fib_n;
//      bigint::bigint fib_n_1;
//      bigint::bigint fib_n_2;
//      ui64 n{};
//
//      fib_n_1 = 1;
//      fib_n_2 = 0;
//
//      for (n = 2; n <= N; ++n) {
//        fib_n = fib_n_1 + fib_n_2;
//        fib_n_2 = fib_n_1;
//        fib_n_1 = fib_n;
//      }
//      return fib_n_1;
//    };

//    [[maybe_unused]]
//    auto
//    pfib = [&N]() {
//      bigint::bigint *pfib_n = new bigint::bigint;
//      bigint::bigint *pfib_n_1 = new bigint::bigint;
//      bigint::bigint *pfib_n_2 = new bigint::bigint;
//      ui64 n{};
//
//      *pfib_n_1 = 1;
//      *pfib_n_2 = 0;
//
//      for (n = 2; n <= N; ++n) {
//        *pfib_n = *pfib_n_1 + *pfib_n_2;
//        std::swap(pfib_n_2, pfib_n);
//        std::swap(pfib_n_2, pfib_n_1);
//      }
//      return *pfib_n_1;
//    };

    [[maybe_unused]]
    auto
    upfib = [&N]() {
      std::unique_ptr<bigint::bigint> upfib_n = bigint::createUniquePtr();
      std::unique_ptr<bigint::bigint> upfib_n_1 = bigint::createUniquePtr();
      std::unique_ptr<bigint::bigint> upfib_n_2 = bigint::createUniquePtr();
      ui64 n{};

      *upfib_n_1 = 1;
      *upfib_n_2 = 0;

      for (n = 2; n <= N; ++n) {
        *upfib_n = *upfib_n_1 + *upfib_n_2;
        std::swap(upfib_n_2, upfib_n);
        std::swap(upfib_n_2, upfib_n_1);
      }
      return *upfib_n_1;
    };

//    bigint::bigint fN;

    if ( bigint::bigint fN; false == testAlgo )
    {
      fN = upfib();

      std::string fileName{"fib-" + std::to_string(N) + ".txt"};
      utilities::cfile_helper::cfile_helper fs = utilities::cfile_helper::cfile_helper(fileName,
                                                                                utilities::cfile_helper::cfile_helper::fstream_direction::fs_OUTPUT);
      fs.get_fstream() << fN;

      std::cout << "[" << __func__ << "] "
                << "fib("
                << N
                << ") of length "
                << bigint::numberOfDigits(fN)
                << " digits written to file "
                << fileName
                << std::endl;
    }
    else
    {
      [[maybe_unused]]
      auto
      upfibTest = [](const ui64 &N)
      {
        std::unique_ptr<bigint::bigint> upfib_n = bigint::createUniquePtr();
        std::unique_ptr<bigint::bigint> upfib_n_1 = bigint::createUniquePtr();
        std::unique_ptr<bigint::bigint> upfib_n_2 = bigint::createUniquePtr();
        ui64 n{};

        *upfib_n_1 = 1;
        *upfib_n_2 = 0;

        for (n = 2; n <= N; ++n)
        {
          *upfib_n = *upfib_n_1 + *upfib_n_2;
          std::swap(upfib_n_2, upfib_n);
          std::swap(upfib_n_2, upfib_n_1);
        }
        return *upfib_n_1;
      };

      bigint::bigint fNnew;
      for (ui64 n{94}; n <= N; ++n)
      {
        fN = upfibTest(n);
        fNnew = fastFib(n, true);

        if (fN != fNnew)
        {
          std::cout << n << ": " << "DIFFERENT" << std::endl;

          std::stringstream olds{};
          std::stringstream news{};
          olds << fN;
          news << fNnew;
          if (olds.str().size() != news.str().size())
          {
            std::cout << n << ": " << "LENGTHS DIFFER" << std::endl;
          }
          exit(2);

//          for (size_t i{0}; i < olds.str().size(); ++i)
//          {
//            if (olds.str()[i] != news.str()[i]) {
//              std::cout << "differ at index "
//                        << i
//                        << " -> '"
//                        << static_cast<char>(olds.str()[i])
//                        << "' - '"
//                        << static_cast<char>(news.str()[i])
//                        << "'"
//                        << std::endl;
//            }
        }
//        else
//        {
//          std::cout << n << ": " << "EQUAL" << std::endl;
//        }
      }
      std::cout << "[" << __func__ << "] "
                << "fib("
                << N
                << ") was computed correctly"
                << std::endl;
    }
  }
  return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  GeneralizedEigenSolver<MatrixXf> ges;
MatrixXf A = MatrixXf::Random(4,4);
MatrixXf B = MatrixXf::Random(4,4);
ges.compute(A, B);
cout << "The (complex) numerators of the generalzied eigenvalues are: " << ges.alphas().transpose() << endl;
cout << "The (real) denominatore of the generalzied eigenvalues are: " << ges.betas().transpose() << endl;
cout << "The (complex) generalzied eigenvalues are (alphas./beta): " << ges.eigenvalues().transpose() << endl;

  return 0;
}