/* -*- 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


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * example_SE3.cpp
 *
 *  Created on: Feb 12, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include <iostream>
#include <Eigen/LU> // for inverse and determinant
#include <cmath>
#include "mrob/SE3.hpp"
#include "mrob/SO3.hpp"

int main()
{

    // TODO please write me as a test unit!!!
    // SO3 tests
    // ========================================================

    // Testing the Identity element
    {
    mrob::SO3 R;
    R.print();
    std::cout << "Identity element error= " << R.ln_vee().norm() << std::endl;
    }

    // Testing the Identity element plus epsilon
    {
    Mat31 w;
    w << 1e-15, 0, 0;
    mrob::SO3 R = mrob::SO3(w);
    std::cout << "Identity element error epsilon = " << R.ln_vee().norm() << std::endl;
    }

    // Testing Regular
    {
    Mat31 w;
    w << 1.2, -0.3, 0.2;
    mrob::SO3 R = mrob::SO3(w);
    std::cout << "Exponent and Log test " << (R.ln_vee() - w).norm() << std::endl;
    }

    //testing bool function isSO3
    {
    Mat31 w;
    w << 1.2, -0.3, 0.2;
    mrob::SO3 R(w);
    std::cout << "Matris is SO3: " << mrob::isSO3(R.R()) << std::endl;
    Mat3 notR = Mat3::Random();
    std::cout << "Matris is SO3: " << mrob::isSO3(notR) << std::endl;
    }

    // Testing Pi
    {
    Mat31 w;
    w << M_PI, 0.0, 0.0;
    mrob::SO3 R = mrob::SO3(w);
    //R.print();
    std::cout << "Pi rotation 1 component = " << (R.ln_vee() - w).norm() << std::endl;
    }

    {
    Mat31 w;
    w << M_PI*std::sqrt(1.0/3), M_PI*std::sqrt(1.0/3), M_PI*std::sqrt(1.0/3);
    mrob::SO3 R = mrob::SO3(w);
    std::cout << "Pi rotation 3 components= " <<  (R.ln_vee() - w).norm() << std::endl;
    }

    // testing operators}
    {
    std::cout << "\ntesting operators\n";
    Mat31 w;
    w << M_PI, 0.0, 0.0;
    mrob::SO3 R = mrob::SO3(w);
    R.print();
    Mat3 w_hat = R.ln();
    std::cout << w_hat << std::endl;

    std::cout << "testing inverse"  << std::endl;
    mrob::SO3 Rt = R.inv();
    std::cout << "invers = " << Rt.R()  << std::endl;
    }

    // SE3 tests
    // ========================================================
    // testing the constructor
    std::cout << "\n\nSE3 tests"  << std::endl;
    {
    mrob::SE3 T;
    //T.print();
    std::cout << "Identity element error= " << T.ln_vee().norm() << std::endl;
    }
    {
    Mat61 xi;
    xi << 1e-9,0,0, 20, 100, 4;
    mrob::SE3 T(xi);
    T.print();
    std::cout << "random element plus epsilon= " << (T.ln_vee() - xi).norm() << std::endl;
    }
    {
    Mat61 xi;
    xi << 1,0,-0.2, 5, 10, 2;
    mrob::SE3 T(xi);
    //T.print();
    std::cout << "Some normal element error = " << (T.ln_vee() - xi).norm() << std::endl;
    }

    //testing bool function isSE3
    {
    Mat61 xi;
    xi << 1,0,-0.2, 5, 10, 2;
    mrob::SE3 T(xi);
    std::cout << "input SE3 and Matris is SE3?: " << mrob::isSE3(T.T()) << std::endl;
    Mat4 notT = Mat4::Random();
    std::cout << "Random Matris is SE3: " << mrob::isSE3(notT) << std::endl;
    }
    {
    Mat61 xi;
    xi << M_PI,0,0, 5, 100, 2;
    mrob::SE3 T(xi);
    T.print_lie();
    std::cout << "Pi error plus trans= " << (T.ln_vee()-xi).norm() << std::endl;
    }
    {
    Mat61 xi;
    xi << M_PI*std::sqrt(1.0/3)-1e-4, M_PI*std::sqrt(1.0/3), M_PI*std::sqrt(1.0/3), 5, 1000, 200;
    mrob::SE3 T(xi);
    T.print_lie();
    std::cout << "Pi error 3 comp plus trans= " << (T.ln_vee()-xi).norm() << std::endl;
    // The error ind
    }
    mrob::SE3 T1;
    T1.print();
    T1.print_lie();
    Mat61 xi;
    xi << 1,0,-0.2, 5, 10, 2;
    mrob::SE3 T(xi);
    T.print();
    xi << T.ln_vee();
    mrob::SE3 T2(xi);
    T2.print();
    std::cout << "Matrix distance = " << (T.T()-T2.T()).norm() << std::endl;

    std::cout << "testing update\n";
    T2.update_lhs(xi);
    T2.print();
    Mat41 v;
    v << 1.0, 3.2, -1.2, 1.0;
    std::cout << T2.T()*v << std::endl;
    v = T2.T()*v;
    std::cout << v << std::endl;

    std::cout << "testing inverse"  << std::endl;
    mrob::SE3 Tt = T2.inv();
    std::cout << "invers = " << Tt.T()  << std::endl;
    std::cout << "Matrix distance = " << (Tt.T()-T2.T().inverse()).norm() << std::endl;
    Mat61 xi2 = -T2.ln_vee();
    mrob::SE3 T22( xi2);
    std::cout << "Matrix distance by negating= " << (T22.T()-T2.T().inverse()).norm() << std::endl;

    std::cout << "testing adjoint"  << std::endl;
    std::cout << "Adjoint= " << Tt.adj()  << std::endl;

    // testing subblock matrices
    std::cout << "Subblock methods an matrix:"  << std::endl;
    T.print();
    std::cout << "\nT = " << T.T()  << std::endl;
    std::cout << "\nR = " << T.R()  << std::endl;
    std::cout << "\nt = " << T.t()  << std::endl;

    // Acess inner matrix
    T.ref2T() << 1, 0, 0, 0,
             0, 1, 0, 0,
             0, 0, 5, 0,
             0, 0, 0 , 1;
    T.print();

    // testing transformation
    {
    Mat61 xi;
    xi << 1,0,-0.2, 5, 10, 2;
    mrob::SE3 T(xi);
    Mat31 p,p2;
    p << 2,3,-1;
    p2 = p;
    p = T.transform(p);
    p = T.inv().transform(p);
    std::cout << "Testing transformations = " << (p-p2).norm() << std::endl;
    }
    //testing SE3 multiplications

}


/*

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;
}


/* Boost test/test_float.cpp
 * test arithmetic operations on a range of intervals
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/config.hpp>
#include "bugs.hpp"

/* All the following tests should be BOOST_CHECK; however, if a test fails,
   the probability is high that hundreds of other tests will fail, so it is
   replaced by BOOST_REQUIRE to avoid flooding the logs. */

template<class T, class F>
void test_unary() {
  typedef typename F::I I;
  for(I a(-10., -9.91); a.lower() <= 10.; a += 0.3) {
    if (!F::validate(a)) continue;
    I rI = F::f_I(a);
    T rT1 = F::f_T(a.lower()), rT2 = F::f_T(a.upper()),
      rT3 = F::f_T(median(a));
    BOOST_REQUIRE(in(rT1, rI));
    BOOST_REQUIRE(in(rT2, rI));
    BOOST_REQUIRE(in(rT3, rI));
  }
}

template<class T, class F>
void test_binary() {
  typedef typename F::I I;
  for(I a(-10., -9.91); a.lower() <= 10.; a += 0.3) {
    for(I b(-10., -9.91); b.lower() <= 10.; b += 0.3) {
      if (!F::validate(a, b)) continue;
      T al = a.lower(), au = a.upper(), bl = b.lower(), bu = b.upper();
      I rII = F::f_II(a, b);
      I rIT1 = F::f_IT(a, bl), rIT2 = F::f_IT(a, bu);
      I rTI1 = F::f_TI(al, b), rTI2 = F::f_TI(au, b);
      I rTT1 = F::f_TT(al, bl), rTT2 = F::f_TT(al, bu);
      I rTT3 = F::f_TT(au, bl), rTT4 = F::f_TT(au, bu);
      BOOST_REQUIRE(subset(rTT1, rIT1));
      BOOST_REQUIRE(subset(rTT3, rIT1));
      BOOST_REQUIRE(subset(rTT2, rIT2));
      BOOST_REQUIRE(subset(rTT4, rIT2));
      BOOST_REQUIRE(subset(rTT1, rTI1));
      BOOST_REQUIRE(subset(rTT2, rTI1));
      BOOST_REQUIRE(subset(rTT3, rTI2));
      BOOST_REQUIRE(subset(rTT4, rTI2));
      BOOST_REQUIRE(subset(rIT1, rII));
      BOOST_REQUIRE(subset(rIT2, rII));
      BOOST_REQUIRE(subset(rTI1, rII));
      BOOST_REQUIRE(subset(rTI2, rII));
    }
  }
}

#define new_unary_bunch(name, op, val) \
  template<class T> \
  struct name { \
    typedef boost::numeric::interval<T> I; \
    static I f_I(const I& a) { return op(a); } \
    static T f_T(const T& a) { return op(a); } \
    static bool validate(const I& a) { return val; } \
  }

//#ifndef BOOST_NO_STDC_NAMESPACE
using std::abs;
using std::sqrt;
//#endif

new_unary_bunch(bunch_pos, +, true);
new_unary_bunch(bunch_neg, -, true);
new_unary_bunch(bunch_sqrt, sqrt, a.lower() >= 0.);
new_unary_bunch(bunch_abs, abs, true);

template<class T>
void test_all_unaries() {
  BOOST_CHECKPOINT("pos");  test_unary<T, bunch_pos<T> >();
  BOOST_CHECKPOINT("neg");  test_unary<T, bunch_neg<T> >();
  BOOST_CHECKPOINT("sqrt"); test_unary<T, bunch_sqrt<T> >();
  BOOST_CHECKPOINT("abs");  test_unary<T, bunch_abs<T> >();
}

#define new_binary_bunch(name, op, val) \
  template<class T> \
  struct bunch_##name { \
    typedef boost::numeric::interval<T> I; \
    static I f_II(const I& a, const I& b) { return a op b; } \
    static I f_IT(const I& a, const T& b) { return a op b; } \
    static I f_TI(const T& a, const I& b) { return a op b; } \
    static I f_TT(const T& a, const T& b) \
    { return boost::numeric::interval_lib::name<I>(a,b); } \
    static bool validate(const I& a, const I& b) { return val; } \
  }

new_binary_bunch(add, +, true);
new_binary_bunch(sub, -, true);
new_binary_bunch(mul, *, true);
new_binary_bunch(div, /, !zero_in(b));

template<class T>
void test_all_binaries() {
  BOOST_CHECKPOINT("add"); test_binary<T, bunch_add<T> >();
  BOOST_CHECKPOINT("sub"); test_binary<T, bunch_sub<T> >();
  BOOST_CHECKPOINT("mul"); test_binary<T, bunch_mul<T> >();
  BOOST_CHECKPOINT("div"); test_binary<T, bunch_div<T> >();
}

int test_main(int, char *[]) {
  BOOST_CHECKPOINT("float tests");
  test_all_unaries<float> ();
  test_all_binaries<float> ();
  BOOST_CHECKPOINT("double tests");
  test_all_unaries<double>();
  test_all_binaries<double>();
  //BOOST_CHECKPOINT("long double tests");
  //test_all_unaries<long double>();
  //test_all_binaries<long double>();
# ifdef __BORLANDC__
  ::detail::ignore_warnings();
# endif
  return 0;
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_STATS_EXCESS_HPP_INCLUDED
#define DHIST_STATS_EXCESS_HPP_INCLUDED 1

#include <boost/python.hpp>

#include <ndhist/ndhist.hpp>
#include <ndhist/stats/kurtosis.hpp>

namespace ndhist {
namespace stats {

namespace detail {

template <typename AxisValueType, typename WeightValueType>
double
calc_axis_excess_impl(
    ndhist const & h
  , intptr_t const axis
)
{
    return calc_axis_kurtosis_impl<AxisValueType, WeightValueType>(h, axis) - 3;
}

}// namespace detail

namespace py {

/**
 * @brief Calculates the excess kurtosis along the given axis of the given
 *     ndhist object. As in statistics, the excess kurtosis is defined as
 *     :math:`Excess[x] = Kurtosis[x] - 3`.
 *     This function generates a projection along the given axis and then
 *     calculates the excess kurtosis.
 *     If None is given as axis, the excess kurtosis for all individual axes
 *     of the ndhist object will be calculated and returned as a tuple.
 *     But if the dimensionality of the ndhist object is 1, a scalar value is
 *     returned.
 *
 * @note This function is only defined for ndhist objects with POD axis values
 *     AND POD weight values.
 */
boost::python::object
excess(
    ndhist const & h
  , boost::python::object const & axis = boost::python::object()
);

}// namespace py

}// namespace stats
}// namespace ndhist

#endif // !DHIST_STATS_EXCESS_HPP_INCLUDED


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

using namespace std;
using namespace Eigen;

int main()
{
  MatrixXf m(3,3), n(2,2);

  m << 1,2,3,
       4,5,6,
       7,8,9;

  // assignment through a block operation,
  //  block as rvalue
  n = m.block(0,0,2,2);

  //print n
  cout << "n = " << endl << n << endl << endl;

  n << 1,1,
       1,1;

  // block as lvalue
  m.block(0,0,2,2) = n;

  //print m
  cout << "m = " << endl << m << endl;
}


/*! \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);
}


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include <doctest/doctest.h>  // for ResultBuilder

#include <array>                             // for operator==
#include <boost/multiprecision/cpp_int.hpp>  // for cpp_int
#include <ostream>                           // for operator<<
#include <tuple>                             // for get, tuple
#include <type_traits>                       // for move

#include "projgeom/ck_plane.hpp"       // for ellck, hyck
#include "projgeom/common_concepts.h"  // for Value_type
#include "projgeom/fractions.hpp"      // for operator*
#include "projgeom/pg_common.hpp"      // for cross
#include "projgeom/pg_line.hpp"        // for pg_line
#include "projgeom/pg_object.hpp"      // for operator*
#include "projgeom/pg_point.hpp"       // for pg_point
#include "projgeom/proj_plane.hpp"     // for tri_dual
// #include <iostream>

using namespace fun;

static const auto Zero = doctest::Approx(0).epsilon(0.01);

/**
 * @brief
 *
 * @param[in] a
 * @return true
 * @return false
 */
template <typename T> inline auto ApproxZero(const T& a) -> bool {
    return a[0] == Zero && a[1] == Zero && a[2] == Zero;
}

/**
 * @brief
 *
 * @tparam PG
 * @param[in] myck
 */
template <typename PG> void chk_tri(const PG& myck) {
    using Point = typename PG::point_t;
    using K = Value_type<Point>;

    auto a1 = Point{1, 3, 1};
    auto a2 = Point{4, 2, 1};
    auto a3 = Point{1, 1, -1};
    auto a4 = plucker(2, a1, 3, a2);

    const auto triangle = std::tuple{std::move(a1), std::move(a2), std::move(a3)};
    const auto trilateral = tri_dual(triangle);
    const auto& [l1, l2, l3] = trilateral;

    const auto Q = myck.tri_quadrance(triangle);
    const auto S = myck.tri_spread(trilateral);

    if constexpr (Integral<K>) {
        CHECK(myck.perp(myck.perp(a4)) == a4);
        CHECK(myck.perp(myck.perp(l1)) == l1);
        CHECK(myck.perp(myck.perp(l2)) == l2);
        CHECK(myck.perp(myck.perp(l3)) == l3);
        // CHECK(check_cross_law(S, std::get<2>(Q)) == K(0));
        // CHECK(check_cross_law(Q, std::get<2>(S)) == K(0));
    } else {
        CHECK(ApproxZero(cross(myck.perp(myck.perp(a4)), a4)));
        CHECK(ApproxZero(cross(myck.perp(myck.perp(l1)), l1)));
        CHECK(ApproxZero(cross(myck.perp(myck.perp(l2)), l2)));
        CHECK(ApproxZero(cross(myck.perp(myck.perp(l3)), l3)));
        CHECK(check_cross_law(S, std::get<2>(Q)) == Zero);
        CHECK(check_cross_law(Q, std::get<2>(S)) == Zero);
    }
}

/**
 * @brief
 *
 * @tparam PG
 * @param[in] myck
 */
template <typename PG> void chk_tri2(const PG& myck) {
    using Point = typename PG::point_t;
    using K = Value_type<Point>;

    auto a1 = Point{1, 3, 1};
    auto a2 = Point{4, 2, 1};
    auto a4 = plucker(2, a1, 3, a2);

    const auto collin = std::tuple{std::move(a1), std::move(a2), std::move(a4)};
    const auto Q2 = myck.tri_quadrance(collin);

    if constexpr (Integral<K>) {
        CHECK(check_cross_TQF(Q2) == 0);
    } else {
        CHECK(check_cross_TQF(Q2) == Zero);
    }
}

TEST_CASE("Elliptic/Hyperbolic plane") {
    using boost::multiprecision::cpp_int;

    chk_tri(ellck<pg_point<cpp_int>>());
    chk_tri(ellck<pg_line<cpp_int>>());
    chk_tri(hyck<pg_point<cpp_int>>());
    chk_tri(hyck<pg_line<cpp_int>>());

    chk_tri2(ellck<pg_point<cpp_int>>());
    chk_tri2(ellck<pg_line<cpp_int>>());
    chk_tri2(hyck<pg_point<cpp_int>>());
    chk_tri2(hyck<pg_line<cpp_int>>());
}

TEST_CASE("Elliptic/Hyperbolic plane (double)") {
    chk_tri(ellck<pg_point<double>>());
    chk_tri(ellck<pg_line<double>>());
    chk_tri(hyck<pg_point<double>>());
    chk_tri(hyck<pg_line<double>>());

    chk_tri2(ellck<pg_point<double>>());
    chk_tri2(ellck<pg_line<double>>());
    chk_tri2(hyck<pg_point<double>>());
    chk_tri2(hyck<pg_line<double>>());
}


/* 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


#include <igl/active_set.h>
#include <igl/boundary_facets.h>
#include <igl/cotmatrix.h>
#include <igl/invert_diag.h>
#include <igl/jet.h>
#include <igl/massmatrix.h>
#include <igl/readOFF.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <iostream>
#include "tutorial_shared_path.h"

Eigen::VectorXi b;
Eigen::VectorXd B,bc,lx,ux,Beq,Bieq,Z;
Eigen::SparseMatrix<double> Q,Aeq,Aieq;

void solve(igl::opengl::glfw::Viewer &viewer)
{
  using namespace std;
  igl::active_set_params as;
  as.max_iter = 8;
  igl::active_set(Q,B,b,bc,Aeq,Beq,Aieq,Bieq,lx,ux,as,Z);
  // Pseudo-color based on solution
  Eigen::MatrixXd C;
  igl::jet(Z,0,1,C);
  viewer.data().set_colors(C);
}

bool key_down(igl::opengl::glfw::Viewer &viewer, unsigned char key, int mod)
{
  switch(key)
  {
    case '.':
      Beq(0) *= 2.0;
      solve(viewer);
      return true;
    case ',':
      Beq(0) /= 2.0;
      solve(viewer);
      return true;
    case ' ':
      solve(viewer);
      return true;
    default:
      return false;
  }
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  MatrixXd V;
  MatrixXi F;
  igl::readOFF(TUTORIAL_SHARED_PATH "/cheburashka.off",V,F);

  // Plot the mesh
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().show_lines = false;
  viewer.callback_key_down = &key_down;

  // One fixed point
  b.resize(1,1);
  // point on belly.
  b<<2556;
  bc.resize(1,1);
  bc<<1;

  // Construct Laplacian and mass matrix
  SparseMatrix<double> L,M,Minv;
  igl::cotmatrix(V,F,L);
  igl::massmatrix(V,F,igl::MASSMATRIX_TYPE_VORONOI,M);
  //M = (M/M.diagonal().maxCoeff()).eval();
  igl::invert_diag(M,Minv);
  // Bi-Laplacian
  Q = L.transpose() * (Minv * L);
  // Zero linear term
  B = VectorXd::Zero(V.rows(),1);

  // Lower and upper bound
  lx = VectorXd::Zero(V.rows(),1);
  ux = VectorXd::Ones(V.rows(),1);

  // Equality constraint constrain solution to sum to 1
  Beq.resize(1,1);
  Beq(0) = 0.08;
  Aeq = M.diagonal().sparseView().transpose();
  // (Empty inequality constraints)
  solve(viewer);
  cout<<
    "Press '.' to increase scale and resolve."<<endl<<
    "Press ',' to decrease scale and resolve."<<endl;

  viewer.launch();
}


/**
 * @file engquistoshernumericalflux_main.cc
 * @brief NPDE homework "EngquistOsherNumericalFlux" code
 * @author Oliver Rietmann
 * @date 23.04.2019
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <cstdlib>
#include <fstream>
#include <iostream>

#include "engquistoshernumericalflux.h"

/* SAM_LISTING_BEGIN_1 */
const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

double u0(double x) { return (0.0 < x && x <= 1.0) ? 1.0 : -1.0; }

int main() {
  unsigned int N = 100;
  double a = -1.2;
  double b = 2.2;
  double T = 1.0;
  double h = (b - a) / N;

  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N, a - 0.5 * h, b - 0.5 * h);
  Eigen::VectorXd uinitial = x.unaryExpr(std::ref(u0));
  Eigen::VectorXd ufinal =
      EngquistOsherNumericalFlux::solveCP(a, b, uinitial, T);

  //====================
  // Your code goes here
  // Use std::ofstream to write the solution to
  // the file "ufinal.csv". To plot this file
  // you may uncomment the following line:
  // std::system("python3 " CURRENT_SOURCE_DIR "/plot_solution.py "
  // CURRENT_BINARY_DIR "/ufinal.csv " CURRENT_BINARY_DIR "/ufinal.eps");
  //====================

  return 0;
}
/* SAM_LISTING_END_1 */


#include "math/Base.h"
#include "operation/field/setup.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(Base)

BOOST_AUTO_TEST_CASE(intlog) {
  unsigned int p2w = math::intlog(0);
  BOOST_CHECK_EQUAL(p2w, 0);
  p2w = math::intlog(1);
  BOOST_CHECK_EQUAL(p2w, 0);
  p2w = math::intlog(2);
  BOOST_CHECK_EQUAL(p2w, 1);
  for (unsigned int i = 0; i < sizeof(int) * 8; ++i) {
    p2w = math::intlog(1 << i);
    BOOST_CHECK_EQUAL(p2w, i);
  }
  for (unsigned int i = 1; i < sizeof(int) * 8; ++i) {
    p2w = math::intlog((1 << i) + 1);
    BOOST_CHECK_EQUAL(p2w, i + 1);
  }
  for (unsigned int i = 2; i < sizeof(int)*8; i++) {
    p2w = math::intlog((1 << i) - 1);
    BOOST_CHECK_EQUAL(p2w, i);
  }
  // Check when every bit is 1
  p2w = math::intlog(static_cast<unsigned>(-1));
  BOOST_CHECK_EQUAL(p2w, sizeof(int) * 8);
}

BOOST_AUTO_TEST_SUITE_END()

bool init_unit_test() {
  return true;
}