/*
   The MIT License (MIT)

   Copyright (c) 2011 - 2013, Philipp Heise and Sebastian Klose

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.
*/

#include <cvt/math/SE3.h>
#include <cvt/math/Math.h>
#include <cvt/util/CVTTest.h>
#include <cvt/vision/Vision.h>

#include <Eigen/Core>

namespace cvt {

    static bool testJacobian()
    {
        Eigen::Matrix<double, 6, 1> parameter = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 3, 6> jNumeric, jAnalytic, jDiff;

        parameter[ 0 ] = Math::deg2Rad(  3 );
        parameter[ 1 ] = Math::deg2Rad( -6 );
        parameter[ 2 ] = Math::deg2Rad(  5 );
        parameter[ 3 ] = 20;
        parameter[ 4 ] = 30;
        parameter[ 5 ] = 40;

        SE3<double> pose;
        pose.set( parameter );

        Eigen::Matrix<double, 3, 1> point, ppoint;
        Eigen::Matrix<double, 3, 1> p, pp;
        point[ 0 ] = 13; point[ 1 ] = 8; point[ 2 ] = 12;;

        pose.transform( p, point );

        double h = 0.0001;
        for( size_t i = 0; i < 6; i++ ){
            delta[ i ] = h;

            pose.apply( delta );

            pose.transform( pp, point );
            jNumeric.col( i ) = ( pp - p ) / h;

            delta[ i ] = 0;
            pose.set( parameter );
        }

        pose.jacobian( jAnalytic, point );

        bool b, ret = true;
        jDiff = jAnalytic - jNumeric;

        b = ( jDiff.array().abs().sum() / 18.0 ) < 0.001;

        CVTTEST_PRINT( "Pose Jacobian", b );
        if( !b ){
            std::cout << "Analytic:\n" << jAnalytic << std::endl;
            std::cout << "Numeric:\n" << jNumeric << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        return ret;
    }

    static bool testHessian()
    {
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 24, 6> hN, hA;

        SE3<double> pose;
        pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );

        Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
        K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
        K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
        K( 2, 2 ) = 1.0;

        Eigen::Matrix<double, 3, 1> point;
        Eigen::Matrix<double, 3, 1> p, ff, fb, bf, bb, xxf, xxb, hess;
        point[ 0 ] = 16;
        point[ 1 ] = 80;
        point[ 2 ] = 13;

        pose.transform( p, point );

        double h = 0.0001;
        for( size_t i = 0; i < 6; i++ ){
            for( size_t j = 0; j < 6; j++ ){
                delta.setZero();
                if( i == j ){
                    // +
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( xxf, point );
                    pose.apply( -delta );

                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( xxb, point );
                    pose.apply( -delta );

                    hess = ( xxb - 2 * p + xxf ) / ( h*h );
                } else {
                    delta[ i ] = h;
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( ff, point );
                    pose.apply( -delta );

                    delta[ i ] = h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( fb, point );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] =  h;
                    pose.apply( delta );
                    pose.transform( bf, point );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( bb, point );
                    pose.apply( -delta );

                    hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
                }

                hN( 4 * i , j ) = hess[ 0 ];
                hN( 4 * i + 1 , j ) = hess[ 1 ];
                hN( 4 * i + 2 , j ) = hess[ 2 ];
                hN( 4 * i + 3 , j ) = 0.0;
            }
        }

        pose.hessian( hA, p );

        bool b, ret = true;
        Eigen::Matrix<double, 24, 6> jDiff;
        jDiff = hN - hA;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.00001;

        CVTTEST_PRINT( "Pose Hessian", b );
        if( !b ){
            std::cout << "Analytic:\n" << hA << std::endl;
            std::cout << "Numeric:\n" << hN << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        return ret;
    }

    static void projectWithCam( Eigen::Matrix<double, 2, 1> & sp,
                                const Eigen::Matrix<double, 3, 1> & p3d,
                                const Eigen::Matrix<double, 3, 3> & K )
    {
        sp[ 0 ] = K( 0, 0 ) * p3d.x() / p3d.z() + K( 0, 2 );
        sp[ 1 ] = K( 1, 1 ) * p3d.y() / p3d.z() + K( 1, 2 );
    }

    static bool testScreenJacobian()
    {
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 2, 6> shNumeric, sh;

        SE3<double> pose;
        pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );

        Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
        K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
        K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
        K( 2, 2 ) = 1.0;

        Eigen::Matrix<double, 3, 1> point, ptrans;
        Eigen::Matrix<double, 2, 1> sp, ff, bb, jac;
        point[ 0 ] = 100; point[ 1 ] = 200; point[ 2 ] = 300;

        // project the point with current parameters
        pose.transform( ptrans, point );
        projectWithCam( sp, ptrans, K );

        double h = 0.001;
        for( size_t i = 0; i < 6; i++ ){
            delta[ i ] = h;
            pose.apply( delta );
            pose.transform( ptrans, point );
            projectWithCam( ff, ptrans, K );
            pose.apply( -delta );

            delta[ i ] = -h;
            pose.apply( delta );
            pose.transform( ptrans, point );
            projectWithCam( bb, ptrans, K );
            pose.apply( -delta );

            jac = ( ff - bb ) / ( 2 * h );
            delta.setZero();

            shNumeric( 0, i ) = jac[ 0 ];
            shNumeric( 1, i ) = jac[ 1 ];

        }

        pose.transform( ptrans, point );
        pose.screenJacobian( sh, ptrans, K );

        bool b, ret = true;
        Eigen::Matrix<double, 2, 6> jDiff;
        jDiff = shNumeric - sh;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;

        CVTTEST_PRINT( "Pose ScreenJacobian", b );
        if( !b ){
            std::cout << "Analytic:\n" << sh << std::endl;
            std::cout << "Numeric:\n" << shNumeric << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        return ret;
    }
    static bool testScreenHessian()
    {
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 6, 6> shNumericX, shNumericY, shX, shY;

        SE3<double> pose;
        pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );

        Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
        K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
        K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
        K( 2, 2 ) = 1.0;

        Eigen::Matrix<double, 3, 1> point, ptrans;
        Eigen::Matrix<double, 2, 1> sp, ff, fb, bf, bb, xxf, xxb, hess;
        point[ 0 ] = 100; point[ 1 ] = 200; point[ 2 ] = 300;

        // project the point with current parameters
        pose.transform( ptrans, point );
        projectWithCam( sp, ptrans, K );

        double h = 0.001;
        for( size_t i = 0; i < 6; i++ ){
            for( size_t j = 0; j < 6; j++ ){

                if( i == j ){
                    // +
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( xxf, ptrans, K );
                    delta[ j ] = -2 * h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( xxb, ptrans, K );

                    hess = ( xxb - 2 * sp + xxf ) / ( h*h );

                    // back to start
                    delta[ j ] = h;
                    pose.apply( delta );
                    delta[ j ] = 0;
                } else {
                    delta[ i ] = h;
                    delta[ j ] = h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( ff, ptrans, K );
                    pose.apply( -delta );

                    delta[ i ] = h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( fb, ptrans, K );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] =  h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( bf, ptrans, K );
                    pose.apply( -delta );

                    delta[ i ] = -h;
                    delta[ j ] = -h;
                    pose.apply( delta );
                    pose.transform( ptrans, point );
                    projectWithCam( bb, ptrans, K );
                    pose.apply( -delta );

                    hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
                    delta.setZero();
                }

                shNumericX( i, j ) = hess[ 0 ];
                shNumericY( i, j ) = hess[ 1 ];

            }
        }

        pose.transform( ptrans, point );
        pose.screenHessian( shX, shY, ptrans, K );

        bool b, ret = true;
        Eigen::Matrix<double, 6, 6> jDiff;
		jDiff = shNumericX - shX;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;

        CVTTEST_PRINT( "Pose ScreenHessian X", b );
        if( !b ){
            std::cout << "Analytic:\n" << shX << std::endl;
            std::cout << "Numeric:\n" << shNumericX << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        jDiff = shNumericY - shY;
		b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.0001;

        CVTTEST_PRINT( "Pose ScreenHessian Y", b );
        if( !b ){
            std::cout << "Analytic:\n" << shY << std::endl;
            std::cout << "Numeric:\n" << shNumericY << std::endl;
            std::cout << "Difference:\n" << jDiff << std::endl;
        }
        ret &= b;

        return ret;
    }

    static bool testApply()
    {
        bool b, ret;
        // apply delta:
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
        Eigen::Matrix<double, 4, 4> diff;

        SE3<double> pose;
        pose.set( delta );
        delta[ 0 ] = Math::deg2Rad( 1.5 );
        delta[ 1 ] = Math::deg2Rad( 1.1 );
        delta[ 2 ] = Math::deg2Rad( 1.6 );
        delta[ 3 ] = 1;
        delta[ 4 ] = 1;
        delta[ 5 ] = 1;
        pose.apply( delta );

        expectedT( 0, 3 ) = delta[ 3 ];
        expectedT( 1, 3 ) = delta[ 4 ];
        expectedT( 2, 3 ) = delta[ 5 ];

        Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
        double angle = axis.norm();	axis /= angle;

        expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
        diff = expectedT - pose.transformation();

        ret = b = ( diff.array().abs().sum() / 12 < 0.001 );

        if( !b ){
            std::cout << expectedT << std::endl;
            std::cout << pose.transformation() << std::endl;
            std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
        }

        pose.apply( -delta );
        expectedT.setIdentity();

        b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
        CVTTEST_PRINT( "apply", b );
        ret &= b;

        return ret;
    }

    static bool testInitAndSet()
    {
        bool ret = true;
        SE3<double> pose;

        bool b;
        b = ( pose.transformation() == Eigen::Matrix<double, 4, 4>::Identity() );
        ret &= b;
        CVTTEST_PRINT( "Initialization", b );

        pose.set( 0, 0, 0, 100, 200, 300 );
        b = ( pose.transformation()( 0, 3 ) == 100 ) &&
            ( pose.transformation()( 1, 3 ) == 200 ) &&
            ( pose.transformation()( 2, 3 ) == 300 ) &&
            ( pose.transformation()( 3, 3 ) ==   1 ) &&
            ( pose.transformation()( 2, 2 ) ==   1 ) &&
            ( pose.transformation()( 1, 1 ) ==   1 ) &&
            ( pose.transformation()( 0, 0 ) ==   1 );

        pose.set( Math::deg2Rad( 90.0 ), 0, 0, 0, 0, 0 );
        b &= ( pose.transformation()( 0, 3 ) ==   0 );
        b &= ( pose.transformation()( 1, 3 ) ==   0 );
        b &= ( pose.transformation()( 2, 3 ) ==   0 );
        b &= ( pose.transformation()( 3, 3 ) ==   1 );
        b &= ( pose.transformation()( 0, 0 ) ==   1 );
        b &= ( pose.transformation()( 0, 1 ) ==   0 );
        b &= ( pose.transformation()( 0, 2 ) ==   0 );
        b &= ( pose.transformation()( 1, 0 ) ==   0 );
        b &= ( pose.transformation()( 2, 0 ) ==   0 );
        b &= ( pose.transformation()( 1, 2 ) + 1 < 0.0000001 );
        b &= ( pose.transformation()( 2, 1 ) - 1 < 0.0000001 );
        b &= ( Math::abs( pose.transformation()( 2, 2 ) ) <  0.0000001 );
        b &= ( Math::abs( pose.transformation()( 1, 1 ) ) <  0.0000001 );

        CVTTEST_PRINT( "Set", b );

        ret &= b;

        return ret;
    }

    static void fillExpMatrix( Eigen::Matrix4d& S, const SE3<double>::ParameterVectorType& delta )
    {
        S( 0, 0 ) =         0;
        S( 0, 1 ) = -delta[ 2 ];
        S( 0, 2 ) =  delta[ 1 ];
        S( 0, 3 ) =  delta[ 3 ];

        S( 1, 0 ) =  delta[ 2 ];
        S( 1, 1 ) =         0;
        S( 1, 2 ) = -delta[ 0 ];
        S( 1, 3 ) =  delta[ 4 ];

        S( 2, 0 ) = -delta[ 1 ];
        S( 2, 1 ) =  delta[ 0 ];
        S( 2, 2 ) =         0;
        S( 2, 3 ) =  delta[ 5 ];

        S( 3, 0 ) = S( 3, 1 ) = S( 3, 2 ) = 0;
        S( 3, 3 ) = 0;
    }

    static bool testExponential()
    {
        SE3<double> pose;

        Eigen::Matrix4d S, expectedExp, closedForm;
        S.setZero(); expectedExp.setZero(), closedForm.setZero();

        SE3<double>::ParameterVectorType delta;

		size_t n = 5000;

		bool b = true;
		while( n-- ){
			delta[ 0 ] = Math::rand( -2.6, 2.6 );
			delta[ 1 ] = Math::rand( -2.6, 2.6 );
			delta[ 2 ] = Math::rand( -2.6, 2.6 );
			delta[ 3 ] = Math::rand( -10.0, 10.0 );
			delta[ 4 ] = Math::rand( -10.0, 10.0 );
			delta[ 5 ] = Math::rand( -10.0, 10.0 );

			fillExpMatrix( S, delta );
			cvt::Math::exponential( S, expectedExp, 10 );
			pose.evalExp( closedForm, delta );
			bool res = ( ( expectedExp - closedForm ).array().abs().sum() / 12 < 0.00001 );

			if( !res ){
				std::cout << "Expected: \n" << expectedExp << std::endl;
				std::cout << "Closed Form: \n" << closedForm << std::endl;
			}
			b &= res;
		}

        CVTTEST_PRINT( "exp()", b );

        return b;
    }

BEGIN_CVTTEST( SE3 )
    bool ret = true;

    ret &= testInitAndSet();
    ret &= testApply();
    ret &= testJacobian();
    ret &= testScreenJacobian();
    ret &= testHessian();
    ret &= testScreenHessian();
    ret &= testExponential();

    return ret;
END_CVTTEST

}


#include <algorithm>
#include <cstdint>
#include <iostream>
#include <random>
#include <string>
#include <iomanip>
#include <chrono>

#include <Eigen/Eigen>
#include "sparse_gemm_problems.h"

using namespace Eigen;

#ifndef ITERS
#define ITERS 10
#endif

template<typename T, int U = ColMajor>
Matrix<T, Dynamic, Dynamic, U> generate_random_matrix(int rows, int cols) {
    return Matrix<T, Dynamic, Dynamic, U>::Random(rows, cols);
}

template<typename T>
Matrix<T, Dynamic, 1> generate_random_vector(int entries) {
    return Matrix<T, Dynamic, 1>::Random(entries);
}

template<typename T, int U>
std::tuple <int, int> time_sparse_gemv(const SparseMatrix<T, U>& sp_A, const Matrix<T, Dynamic, Dynamic, U>& A, const Matrix<T, Dynamic, 1>& B, Matrix<T, Dynamic, 1>& C) {

    // Try dense-dense multiplication
    auto start = std::chrono::steady_clock::now();
    for (int i = 0; i < ITERS; ++i) {
        C = A * B;
        C[0]++;  // dummy instruction to prevent optimizing away prev line
    }
    auto end = std::chrono::steady_clock::now();
    int d_time = static_cast<int>(std::chrono::duration<double, std::micro>(end - start).count() / ITERS);

    // Try sparse-dense multiplication
    start = std::chrono::steady_clock::now();
    for (int i = 0; i < ITERS; ++i) {
        C = sp_A * B;
        C[0]++;  // dummy instruction to prevent optimizing away prev line
    }
    end = std::chrono::steady_clock::now();
    int sp_time = static_cast<int>(std::chrono::duration<double, std::micro>(end - start).count() / ITERS);
    return std::tuple<int, int>(sp_time, d_time);
}

template<typename T, int U, int V, int W>
std::tuple <int, int> time_sparse_gemm(const SparseMatrix<T, U>& sp_A, const Matrix<T, Dynamic, Dynamic, U>& A, const Matrix<T, Dynamic, Dynamic, V>& B, Matrix<T, Dynamic, Dynamic, W>& C) {

    // Try dense-dense multiplication
    auto start = std::chrono::steady_clock::now();
    for (int i = 0; i < ITERS; ++i) {
        C = A * B;
        C(0,0)++;  // dummy instruction to prevent optimizing away prev line
    }
    auto end = std::chrono::steady_clock::now();
    int d_time = static_cast<int>(std::chrono::duration<double, std::micro>(end - start).count() / ITERS);

    // Try sparse-dense multiplication
    start = std::chrono::steady_clock::now();
    for (int i = 0; i < ITERS; ++i) {
        C = sp_A * B;
        C(0,0)++;  // dummy instruction to prevent optimizing away prev line
    }
    end = std::chrono::steady_clock::now();
    int sp_time = static_cast<int>(std::chrono::duration<double, std::micro>(end - start).count() / ITERS);
    return std::tuple<int, int>(sp_time, d_time);
}

template<typename T>
std::tuple<int, int> time_sparse_bench_helper(int m, int n, int k, float sparsity, std::default_random_engine & e, std::uniform_real_distribution<double> & rng) {

    const int U = RowMajor;

    // Note: We've determined empirically that B,C in ColMajor is best for
    // both sparse and dense gemm implementations.
    const int V = ColMajor;
    const int W = ColMajor;

    auto A = generate_random_matrix<T, U>(m, k);
    for (int j = 0; j < k; ++j) {
        for (int i = 0; i < m; ++i) {
            if (rng(e) < sparsity) {
                A(i, j) = 0;
            }
        }
    }
    SparseMatrix<T, U> sp_A = A.sparseView();

    if (n == 1) {
        auto B = generate_random_vector<T>(k);
        auto C = generate_random_vector<T>(m);
        return time_sparse_gemv<T, U>(sp_A, A, B, C);
    }
    else {
        auto B = generate_random_matrix<T, V>(k, n);
        auto C = generate_random_matrix<T, W>(m, n);
        return time_sparse_gemm<T, U, V, W>(sp_A, A, B, C);
    }
}

int main() {

    // Set up RNG
    std::random_device r;
    std::default_random_engine e(r());
    std::uniform_real_distribution<double> rng(0, 1);

    std::cout << std::setw(30) << "Times" << std::endl;
    std::cout << std::setfill('-') << std::setw(110) << "-" << std::endl;
    std::cout << std::setfill(' ');
    std::cout << "    m       n      k      a_t    b_t    sparsity  precision  sparse time (usec) dense time (usec)   speedup " << std::endl;

    std::vector<std::string> types = {"uint8_t", "float"};

    for (const auto &type_name : types) {

        for (const auto &problem : inference_device_set) {

            int m,n,k;
            bool a_t, b_t;
            float sparsity;

            std::tie(m, n, k, a_t, b_t, sparsity) = problem;

            std::cout << std::setw(7) << m;
            std::cout << std::setw(7) << n;
            std::cout << std::setw(7) << k;
            std::cout << std::setw(7) << a_t ? "t" : "n";
            std::cout << std::setw(7) << b_t ? "t" : "n";
            std::cout << std::setw(11) << sparsity;
            std::cout << std::setw(12) << type_name;

            int sp_time, d_time;

            if (type_name == "uint8_t") {
                std::tie(sp_time, d_time) = time_sparse_bench_helper<std::uint8_t>(m, n, k, sparsity, e, rng);
            } else if (type_name == "float") {
                std::tie(sp_time, d_time) = time_sparse_bench_helper<float>(m, n, k, sparsity, e, rng);
            } else {
                throw std::runtime_error("Unsupported type_name");
            }

            std::cout << std::setw(15) << sp_time;
            std::cout << std::setw(15) << d_time;
            std::cout << std::setw(20) << float(d_time)/sp_time;
            std::cout << std::endl;
        }
    }

    return 0;
}


/**
 * @file contourplot_test.cc
 * @brief NPDE homework ContourPlot code
 * @author Oliver Rietmann
 * @date 25.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "../contourplot.h"

#include <gtest/gtest.h>

#include <Eigen/Core>

namespace ContourPlot::test {

constexpr double Square(double x) { return x * x; }

// Leads to isoline F(x) = 2
double F(Eigen::Vector2d x) { return 0.5 * Square(x(0)) + Square(x(1)); };
const Eigen::Vector2d y0(2.0, 0.0);
const double T = 6.0;

// Compute F(x) - 2 along isoline, which should be zero
Eigen::VectorXd errorAlongIsoline(const Eigen::MatrixXd &isolinePoints) {
  int M = isolinePoints.cols();
  Eigen::VectorXd errors(M);
  for (int m = 0; m < M; ++m) {
    Eigen::Vector2d x =
        Eigen::Vector2d(isolinePoints(0, m), isolinePoints(1, m));
    errors(m) = F(x) - 2.0;
  }
  return errors;
}

TEST(ContourPlot, computeIsolinePoints) {
  auto gradF = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    return Eigen::Vector2d(x(0), 2.0 * x(1));
  };
  Eigen::MatrixXd isolinePoints = computeIsolinePoints(gradF, y0, T);
  Eigen::VectorXd errors = errorAlongIsoline(isolinePoints);
  double tol = 1.0e-5;
  ASSERT_NEAR(0.0, errors.lpNorm<Eigen::Infinity>(), tol);
}

TEST(ContourPlot, computeIsolinePointsDQ) {
  Eigen::MatrixXd isolinePointsDQ = computeIsolinePointsDQ(F, y0, T);
  Eigen::VectorXd errors = errorAlongIsoline(isolinePointsDQ);
  double tol = 1.0e-5;
  ASSERT_NEAR(0.0, errors.lpNorm<Eigen::Infinity>(), tol);
}

}  // namespace ContourPlot::test


#include <unistd.h>	// sleep
#include <armadillo>
#include "Functions.h"

using namespace arma;
using namespace std;

/* Description: Parallel Stochastic Iterative Hard Thresholding (StoIHT) algorithm
 to approximate the vector x from measurements u = A*x.
publication:  Linear Convergence of Stochastic Iterative Greedy Algorithms with Sparse Constraints
https://arxiv.org/abs/1407.0088
*/
vec parallel_Sto_IHT(const mat A, const vec y, const int sparsity, const vec prob_vec,
		const unsigned int max_iter, const double gamma, const double tol,
		unsigned int &num_iters, const simulation_parameters simulation_params){
	// signal parameters
	const unsigned int sig_dim = A.n_cols;
	const unsigned int meas_num = A.n_rows;
	const unsigned int num_block = prob_vec.n_elem;
	const unsigned int block_size = meas_num/num_block;

	// initialization of variables that are shared among cores
	vec x_hat(sig_dim,fill::zeros);	// estimation of the signal
	bool done = false;		// flag to check the convergence criteria
	unsigned int i = 0;		// total number of iterations

	// parallel section of the code starts here
	#pragma omp parallel num_threads(simulation_params.num_cores)
	{

	// initializaiotn of variables that are local to each core
	vec x_hat_local(sig_dim,fill::zeros);
	unsigned int selected_block,first_ind_block,last_ind_block;
	mat A_block;			// submatrix of A
	vec y_block,gradient,b;
	uvec sorted_ind,est_supp;

	// iterations to find the solutions
	while(!done){
		// master thread uses the tally vector to check the convergence criteria
		if (omp_get_thread_num() == 0 ){
			// check exit criteria
			if (norm (y - A*x_hat) < tol || i >= max_iter){
				done = true;	// the flag 'done' is shared among the cores
			}
			if (omp_get_num_threads()  > 1){
				continue;
			}
		}

		i++;
		// randomize
		selected_block = floor(randu()*num_block);
		first_ind_block = block_size*selected_block;
		last_ind_block = block_size*(selected_block+1)-1;

		// Proxy
		A_block = A.rows(first_ind_block,last_ind_block);
		y_block = y.subvec(first_ind_block,last_ind_block);
		#pragma omp critical
		{x_hat_local = x_hat;	}	// read the global estimate while memory is locked

		gradient = -2 * A_block.t() *	(y_block - A_block*x_hat_local);
		b = x_hat_local - gradient * gamma/(num_block*prob_vec(selected_block));

		// Identify
		sorted_ind = sort_index(abs(b),"descend");
		est_supp = sorted_ind(span(0,sparsity - 1));

		// Estimate
		x_hat_local.zeros();
		x_hat_local(est_supp) = b(est_supp);

		#pragma omp critical
		{x_hat = x_hat_local;}	//write the global estimate while memory is locked

	}
	}
	// parallel section of the code ends here
	num_iters = i;
	return x_hat;
}

/* Asynchronous StoIHT Iteration
Algorithm 2 in An Asynchronous Parallel Approach to Sparse Recovery
https://arxiv.org/abs/1701.03458*/

uvec Sto_IHT_async_iteration(vec &x_hat,const vec &tally,const mat &A,const vec &y,
 			const int sparsity, const vec prob_vec, const double gamma){
	// randomize
	const unsigned int num_block = prob_vec.n_elem;
	const unsigned int block_size = y.n_elem / num_block;
	const unsigned int selected_block = floor(randu()*num_block);
	const unsigned int first_ind_block = block_size*selected_block;
	const unsigned int last_ind_block = block_size*(selected_block+1)-1;

	// Proxy
	const mat A_block = A.rows(first_ind_block,last_ind_block);
	const vec y_block = y.subvec(first_ind_block,last_ind_block);
	const vec gradient = -2 * A_block.t() *	(y_block - A_block*x_hat);
	const vec b = x_hat - gradient * gamma/(num_block*prob_vec(selected_block));

	x_hat.zeros();  // this variable is local to each core. NOTE: passed by reference
	// Identify using b (local)
	uvec sorted_ind = sort_index(abs(b),"descend");
	const uvec est_supp_local = sorted_ind(span(0,sparsity - 1));
	// Estimate using b (local)
	x_hat(est_supp_local) = b(est_supp_local);

	// Identify using tally (collective)
	sorted_ind = sort_index(tally,"descend");
	const uvec est_supp_collective = sorted_ind(span(0,sparsity - 1));
	// Estimate using tally (collective)
	x_hat(est_supp_collective) = b(est_supp_collective);

	return 	est_supp_local;
}

void update_tally(vec &tally,const uvec est_supp_local,const uvec prev_est_supp,const unsigned int iter_local){
	/* update the tally score according the rules in:
	An Asynchronous Parallel Approach to Sparse Recovery
	https://arxiv.org/abs/1701.03458*/
	tally(est_supp_local) += iter_local;
	if (iter_local >= 2){
		tally(prev_est_supp) -= iter_local-1;
	}
	return;
}

/* Description: Parallel Stochastic Iterative Hard Thresholding (StoIHT) algorithm with tally score to approximate the vector x from measurements u = A*x.
Publication: An Asynchronous Parallel Approach to Sparse Recovery
https://arxiv.org/abs/1701.03458*/

vec tally_Sto_IHT(const mat &A, const vec &y, const int sparsity, const vec prob_vec,
		const unsigned int max_iter, const double gamma,const double tol,
		unsigned int &num_iters, const simulation_parameters simulation_params){
	uvec slow_cores;
	set_slow_cores(slow_cores, simulation_params);

	const unsigned int sig_dim = A.n_cols;

	// initialization of variables that are shared among cores
	vec tally(sig_dim,fill::zeros);		// vector of tally scores
	vec x_hat_total(sig_dim,fill::zeros);	// estimation of the signal
	bool done = false;			// flag to check the convergence criteria
	unsigned int i = 0;			// total number of iterations

	// parallel section of the code starts here
	#pragma omp parallel num_threads(simulation_params.num_cores)
	{
	//#pragma omp single // a single core executes the following line

	// initializaiotn of variables that are local to each core
	uvec prev_est_supp;			// estimated support in previous iteration
	vec x_hat_local(sig_dim,fill::zeros);  	// this is local to each core
	unsigned int iter_local = 0;		// number of iteration for this core

	// iterations to find the solutions
	while(!done){
		// master thread uses the tally vector to check the convergence criteria
		if (omp_get_thread_num() == 0){
			const uvec sorted_ind = sort_index(abs(tally),"descend");
			const uvec est_supp = sorted_ind(span(0,sparsity - 1));
			const mat A_supp = A.cols(est_supp);
			x_hat_local.zeros();
			x_hat_local(est_supp) = solve(A_supp,y);
			if (norm (y - A*x_hat_local) < tol || i >= max_iter){
				x_hat_total = x_hat_local;
				done = true;
			}
		}

		//slow cores sleep for  simulation_params.sleep_slow_cores microseconds
		if (any( slow_cores == omp_get_thread_num()) ){
			usleep(simulation_params.sleep_slow_cores);
		}

		i++;
		iter_local++;

		// update the local estimate of the support
		uvec est_supp_local;
        try{
        est_supp_local = Sto_IHT_async_iteration(x_hat_local, tally, A, y,
            sparsity, prob_vec, gamma);
        }
        catch(std::logic_error)
        {
            // algorithm did not converge
            i = max_iter;
            done = true;
        }
		// Update Tally
        update_tally(tally,est_supp_local,prev_est_supp,iter_local);

		prev_est_supp = est_supp_local;
	}
	}
	// parallel section of the code ends here
	//cout << endl;
	num_iters = i;
	return x_hat_total;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <cmath>
#include <chrono>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

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

using namespace Eigen;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "plot.h"

using namespace cv;

/**
 * Dataset from:
 *
 *   http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip
 *
 * */

// ------------------------------------------------------------------
// parameters
const int boarder = 20;
const int width = 640;
const int height = 480;
const double fx = 481.2f;
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3;
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1);
const double min_cov = 0.1;
const double max_cov = 10;
const double epsilon = 1e-10;
// ------------------------------------------------------------------

inline double getBilinearInterpolatedValue_eigen(const Mat &img, const Eigen::Vector2d &pt) {
    uchar *d = &img.data[int(pt[1]) * img.step + int(pt[0])];
    double xx = pt[0] - floor(pt[0]);
    double yy = pt[1] - floor(pt[1]);
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[img.step]) +
            xx * yy * double(d[img.step + 1])) / 255.0;
}

// ------------------------------------------------------------------

inline Vector3d px2cam(const Vector2d& px) {
    return Vector3d(
        (px(0, 0) - cx) / fx,
        (px(1, 0) - cy) / fy,
        1
    );
}

inline Vector2d cam2px(const Vector3d& p_cam) {
    return Vector2d(
        p_cam(0, 0) * fx / p_cam(2, 0) + cx,
        p_cam(1, 0) * fy / p_cam(2, 0) + cy
    );
}

inline bool inside(const Vector2d &pt) {
    return pt(0, 0) >= boarder && pt(1, 0) >= boarder
           && pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);
// ------------------------------------------------------------------

double ZNCC(const cv::Mat& im1, const Eigen::Vector2d& pt1, const cv::Mat& im2, Eigen::Vector2d& pt2)
{
    // no need to consider block partly outside because of boarder
    // std::vector<double> v1(ncc_area, 0.0), v2(ncc_area, 0.0); // much slower
    double v1[ncc_area], v2[ncc_area];
    double s1 = 0.0, s2 = 0.0;
    int idx = 0;
    for (int i = -ncc_window_size; i <= ncc_window_size; ++i)
    {
        for (int j = -ncc_window_size; j <= ncc_window_size; ++j)
        {
            double val_1 = static_cast<double>(im1.at<uchar>(pt1.y()+i, pt1.x()+j)) / 255;
            Eigen::Vector2d temp_p2 = pt2;
            temp_p2[0] += j;
            temp_p2[1] += i;
            double val_2 = getBilinearInterpolatedValue_eigen(im2, temp_p2);

            s1 += val_1;
            s2 += val_2;
            v1[idx] = val_1;
            v2[idx] = val_2;
            ++idx;
        }
    }

    double mean_1 = s1 / ncc_area;
    double mean_2 = s2 / ncc_area;

    double numerator = 0.0;
    double den1 = 0.0, den2 = 0.0;
    for (int i = 0; i < ncc_area; ++i)
    {
        double zv1 = v1[i] - mean_1;
        double zv2 = v2[i] - mean_2;
        numerator += zv1*zv2;
        den1 += zv1 * zv1;
        den2 += zv2 * zv2;
    }
    auto zncc =  numerator / (std::sqrt(den1 * den2 + epsilon));
    // std::cout << "zncc = " << zncc << "\n";
    return zncc;
}

bool epipolar_search(const cv::Mat& ref, const cv::Mat& cur, const Sophus::SE3d& Tcr, const Eigen::Vector2d& pt, double depth_mu, double depth_sigma2, Eigen::Vector2d& best_pc, Eigen::Vector2d& epipolar_dir)
{
    double depth_sigma = std::sqrt(depth_sigma2);
    double dmax = depth_mu + 3 * depth_sigma;
    double dmin = depth_mu - 3 * depth_sigma;
    dmin = std::max(0.1, dmin);

    Eigen::Vector3d pn((pt.x()-cx) / fx, (pt.y() - cy) / fy, 1.0);
    pn.normalize();
    Eigen::Vector3d P_max = pn * dmax;
    Eigen::Vector3d P_min = pn * dmin;
    Eigen::Vector3d P_mu = pn * depth_mu;

    Eigen::Vector2d pc_max = cam2px(Tcr * P_max);
    Eigen::Vector2d pc_min = cam2px(Tcr * P_min);
    Eigen::Vector2d pc_mu = cam2px(Tcr * P_mu);

    Eigen::Vector2d epipolar_line = pc_max - pc_min;
    epipolar_dir = epipolar_line.normalized();

    double step = 0.7;
    int nb_samples = std::ceil(epipolar_line.norm() / step);

    double half_range = 0.5 * epipolar_line.norm();
    if (half_range > 100) half_range = 100;

    Eigen::Vector2d p = pc_min;
    double best_zncc = -1.0;
    best_pc = pc_mu;

    // for (int i = 0; i < nb_samples; ++i)
    for (double l = -half_range; l<= half_range; l+= 0.7)
    {
        Eigen::Vector2d p = pc_mu + l * epipolar_dir;

        if (p.x() < boarder || p.x() >= width-boarder || p.y() < boarder || p.y() >= height-boarder)
            continue; // p is outside the cur image

        double zncc = ZNCC(ref, pt, cur, p);
        if (zncc > best_zncc)
        {
            best_zncc = zncc;
            best_pc = p;
        }

        // p += epipolar_dir * step;
    }

    // std::cout << best_zncc << "\n";
    if (best_zncc < 0.85)
        return false;
    else
        return true;
}

void update_depth_filter(const Eigen::Vector2d& pr, const Eigen::Vector2d& pc, const Sophus::SE3d& Tcr, const Eigen::Vector2d& epipolar_dir, cv::Mat& depth, cv::Mat& cov2)
{
    Sophus::SE3d Trc = Tcr.inverse();

    Eigen::Vector3d fr = px2cam(pr);
    fr.normalize();
    Eigen::Vector3d fc = px2cam(pc);
    fc.normalize();
    Eigen::Vector3d f2 = Trc.so3() * fc;
    Eigen::Vector3d trc = Trc.translation();

    // Solve the system of equation for triangulating depth
    Eigen::Matrix2d A;
    Eigen::Vector2d b;
    A(0, 0) = fr.dot(fr);
    A(0, 1) = -fr.dot(f2);
    A(1, 0) = f2.dot(fr);
    A(1, 1) = -f2.dot(f2);
    b[0] = fr.dot(trc);
    b[1] = f2.dot(trc);
    Eigen::Vector2d res = A.inverse() * b;
    Eigen::Vector3d P1 = fr * res[0];
    Eigen::Vector3d P2 = trc + fc * res[1];
    Eigen::Vector3d P_est = (P1 + P2) * 0.5;
    double depth_obs = P_est.norm(); //depth obs

    // Estimate depth uncertainty
    Eigen::Vector3d P = fr * depth_obs;
    Eigen::Vector3d a = P - trc;
    Eigen::Vector3d t = trc.normalized();
    double alpha = std::acos(fr.dot(t));
    double beta = std::acos(a.normalized().dot(-t));
    Eigen::Vector2d pc2 = pc + epipolar_dir;
    Eigen::Vector3d fc2 = px2cam(pc2);
    fc2.normalize();
    double beta_2 = std::acos(fc2.dot(-t));
    double gamma = M_PI - alpha - beta_2;
    double d_noise = trc.norm() * std::sin(beta_2) / std::sin(gamma); // sinus law
    double sigma_obs = depth_obs - d_noise;
    double sigma2_obs = sigma_obs * sigma_obs; // sigma2 obs

    // Depth fusion
    double d = depth.at<double>(static_cast<int>(pr.y()), static_cast<int>(pr.x()));
    double sigma2 = cov2.at<double>(static_cast<int>(pr.y()), static_cast<int>(pr.x()));

    double d_fused = (sigma2_obs * d + sigma2 * depth_obs) / (sigma2 + sigma2_obs);
    double sigma2_fused = (sigma2 * sigma2_obs) / (sigma2 + sigma2_obs);

    depth.at<double>(static_cast<int>(pr.y()), static_cast<int>(pr.x())) = d_fused;
    cov2.at<double>(static_cast<int>(pr.y()), static_cast<int>(pr.x())) = sigma2_fused;
}

void update(const cv::Mat& ref, const cv::Mat& cur, const Sophus::SE3d& Tcr, cv::Mat &depth, cv::Mat &cov2)
{
    Eigen::Vector2d pc;
    Eigen::Vector2d epipolar_dir;
    for (int j = boarder; j < width-boarder; ++j)
    {
        for (int i = boarder; i < height-boarder; ++i)
        {
            double depth_mu = depth.at<double>(i, j);
            double depth_sigma2 = cov2.at<double>(i, j);
            if (depth_sigma2 < min_cov || depth_sigma2 > max_cov)
                continue;
            Eigen::Vector2d pr(j, i);
            bool found = epipolar_search(ref, cur, Tcr, pr, depth_mu, depth_sigma2, pc, epipolar_dir);
            if (!found)
                continue;

            // showEpipolarMatch(ref, cur, pr, pc);

            update_depth_filter(pr, pc, Tcr, epipolar_dir, depth, cov2);
        }
    }
    // std::cout << depth << "\n";

}

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // Read dataset
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // Initial depth image
    Mat ref = imread(color_image_files[0], 0); // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;
    double init_cov2 = 3.0;
    Mat depth(height, width, CV_64F, init_depth);
    Mat depth_cov2(height, width, CV_64F, init_cov2);

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // T_C_W * T_W_R = T_C_R
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        update(ref, curr, pose_T_C_R, depth, depth_cov2);
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
        std::cout << "Time used: " << time_used.count() << "s\n";
        evaludateDepth(ref_depth, depth);
        plotDepth(ref_depth, depth);
        plotCur(curr);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;

    return 0;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    std::vector<SE3d> &poses,
    cv::Mat &ref_depth) {
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while (!fin.eof()) {
        // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin >> image;
        double data[7];
        for (double &d:data) fin >> d;

        color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(
            SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if (!fin.good()) break;
    }
    fin.close();

    // load reference depth
    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if (!fin) return false;
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++) {
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }

    return true;
}

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    double ave_depth_error = 0;
    double ave_depth_error_sq = 0;
    int cnt_depth_data = 0;
    for (int y = boarder; y < depth_truth.rows - boarder; y++)
        for (int x = boarder; x < depth_truth.cols - boarder; x++) {
            double error = depth_truth.ptr<double>(y)[x] - depth_estimate.ptr<double>(y)[x];
            ave_depth_error += error;
            ave_depth_error_sq += error * error;
            cnt_depth_data++;
        }
    ave_depth_error /= cnt_depth_data;
    ave_depth_error_sq /= cnt_depth_data;

    cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}


/** @file
 *****************************************************************************

 Arithmetic in the finite field Fp, for prime p of fixed length using
 NTL as the backend.

 *****************************************************************************
 * @author     Samir Menon, Brennan Shacklett, and David J. Wu
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#ifndef NTLFP_TCC_
#define NTLFP_TCC_

#include <cassert>
#include <cstdlib>
#include <cmath>
#include <NTL/ZZ.h>

#include <libff/algebra/fields/fp_aux.tcc>
#include <libff/algebra/fields/field_utils.hpp>

namespace libsnark {

template<unsigned long modulus>
NTLFp_model<modulus>::NTLFp_model()
{
    NTL::ZZ_p::init(mod_zz());
}

template<unsigned long modulus>
NTLFp_model<modulus>::NTLFp_model(long x)
{
    NTL::ZZ_p::init(mod_zz());
    this->value = x;
}

template <unsigned long modulus>
NTLFp_model<modulus>::NTLFp_model(const NTLFp_model &other)
{
    NTL::ZZ_p::init(mod_zz());
    this->value = other.value;
}

template<unsigned long modulus>
bool NTLFp_model<modulus>::operator==(const NTLFp_model& other) const
{
    return (this->value == other.value);
}

template<unsigned long modulus>
bool NTLFp_model<modulus>::operator!=(const NTLFp_model& other) const
{
    return (this->value != other.value);
}

template<unsigned long modulus>
bool NTLFp_model<modulus>::is_zero() const
{
    return NTL::IsZero(this->value);
}

template<unsigned long modulus>
void NTLFp_model<modulus>::print() const
{
    std::cout << *this;
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::zero()
{
    NTL::ZZ_p::init(mod_zz());
    NTLFp_model<modulus> z(NTL::ZZ_p::zero());
    return z;
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::one()
{
    NTL::ZZ_p::init(mod_zz());
    NTLFp_model<modulus> o(NTL::ZZ_p::zero() + 1);
    return o;
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::operator+=(const NTLFp_model<modulus>& other)
{
    NTL::ZZ_p::init(mod_zz());
    this->value += other.value;
    return *this;
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::operator-=(const NTLFp_model<modulus>& other)
{
    NTL::ZZ_p::init(mod_zz());
    this->value -= other.value;
    return *this;
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::operator*=(const NTLFp_model<modulus>& other)
{
    NTL::ZZ_p::init(mod_zz());
    this->value *= other.value;
    return *this;
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::operator^=(const NTLFp_model<modulus>& other)
{
    return this^=other.as_long();
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::operator^=(const libff::bigint<1>& pwr)
{
    return this^=pwr.as_ulong();
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::operator^=(const unsigned long pwr)
{
    NTL::ZZ_p::init(mod_zz());
    this->value = NTL::power(this->value, pwr);
    return (*this);
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator+(const NTLFp_model<modulus>& other) const
{
    NTLFp_model<modulus> r(*this);
    return (r += other);
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator-(const NTLFp_model<modulus>& other) const
{
    NTLFp_model<modulus> r(*this);
    return (r -= other);
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator*(const NTLFp_model<modulus>& other) const
{
    NTLFp_model<modulus> r;
    NTL::mul(r.value, value, other.value);
    r.value = value * other.value;
    return r;
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator^(const NTLFp_model<modulus>& other) const
{
    NTLFp_model<modulus> r(*this);
    return (r ^= other);
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator^(const unsigned long pwr) const
{
    NTLFp_model<modulus> r(*this);
    return (r ^= pwr);
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator^(const libff::bigint<1>& pwr) const
{
    NTLFp_model<modulus> r(*this);
    return (r ^= pwr.as_ulong());
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::operator-() const
{
    NTLFp_model<modulus> r(modulus - this->value);
    return r;
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::squared() const
{
    NTLFp_model<modulus> r(*this);
    return (r *= r);
}

template<unsigned long modulus>
NTLFp_model<modulus>& NTLFp_model<modulus>::invert()
{
    NTL::ZZ_p inverse = 1 / this->value;;
    this->value = inverse;
    return *this;
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::inverse() const
{
    NTLFp_model<modulus> r(*this);
    return r.invert();
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::random_element()
{
    NTL::ZZ_p::init(mod_zz());
    NTLFp_model<modulus> r;
    r.value = NTL::ZZ_p(NTL::RandomBnd(modulus));
    return r;
}

template<unsigned long modulus>
void NTLFp_model<modulus>::get_s_and_t(unsigned long& s, unsigned long& t)
{
    s = modulus - 1;
    t = 0;
    while (s % 2 == 0) {
        s /= 2;
        t++;
    }
}

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::sqrt() const
{
    NTL::ZZ_p::init(mod_zz());
    NTL::ZZ_p sqrt = NTL::to_ZZ_p(NTL::SqrRootMod(NTL::rep(this->value), mod_zz()));
    NTLFp_model<modulus> root;
    root.value = sqrt;

    return root;
}

template<unsigned long modulus>
std::ostream& operator<<(std::ostream &out, const NTLFp_model<modulus> &p)
{
    out << p.value;
    return out;
}

template<unsigned long modulus>
std::istream& operator>>(std::istream &in, NTLFp_model<modulus> &p)
{
    in >> p.value;
    return in;
}

} // libsnark

#endif // NTLFP_TCC_


#include <iostream>
#include <boost/math/special_functions/daubechies_scaling.hpp>
#include <boost/math/special_functions/chebyshev_transform.hpp>

template<typename Real, int p>
void bootstrap()
{
    std::cout << "Computing phi. . .\n";
    auto phi = boost::math::daubechies_scaling<Real, p>();
    std::cout << "Computing Chebyshev transform of phi.\n";
    auto cheb = boost::math::chebyshev_transform(phi, phi.support().first, phi.support().second);
    std::cout << "Number of coefficients = " << cheb.coefficients().size() << "\n";
}

int main()
{
    bootstrap<long double, 9>();
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
// With contributions from Cornelius Steinhardt
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_QR_INCLUDE
#define MTL_MATRIX_QR_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

/// QR-Factorization of matrix A(m x n)
/** Return pair R upper triangel matrix and Q= orthogonal matrix. R and Q are always dense2D **/
template <typename Matrix, typename MatrixQ, typename MatrixR>
void qr(const Matrix& A, MatrixQ& Q, MatrixR& R)
{
	vampir_trace<4013> tracer;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;
    typedef typename Magnitude<value_type>::type      magnitude_type;
    typedef mtl::vector::dense_vector<value_type, vector::parameters<> >       vector_type;

    size_type        ncols = num_cols(A), nrows = num_rows(A),
                     mini= ncols == nrows ? ncols - 1 : (nrows >= ncols ? ncols : nrows);
    magnitude_type   factor= magnitude_type(2);

    Q= 1;
    for (size_type i = 0; i < mini; i++) {
	irange r(i, imax); // Intervals [i, n-1]
	vector_type   w(R[r][i]), v(householder_s(w));

	// R-= 2*v*(v'*R)
	MatrixR Rsub(R[r][r]);
	vector_type tmp(-factor * trans(Rsub) * v);
	rank_one_update(Rsub, v, tmp);

	//update Q: Q-= 2*(v*Q)*v'
	MatrixQ Qsub(Q[iall][r]);
	vector_type qtmp(-factor * Qsub * v);
	rank_one_update(Qsub, qtmp, v);
    } //end for
}

/// QR-Factorization of matrix A(m x n)
template <typename Matrix>
std::pair<mtl::matrix::dense2D<typename Collection<Matrix>::value_type, matrix::parameters<> >,
 	  mtl::matrix::dense2D<typename Collection<Matrix>::value_type, matrix::parameters<> > >
inline qr(const Matrix& A)
{
    mtl::matrix::dense2D<typename Collection<Matrix>::value_type, matrix::parameters<> >  R(A), Q(num_rows(A),num_rows(A));
    qr(A, Q, R);
    return std::make_pair(Q,R);
}

// QR-Factorization of matrix A
// Return Q and R with A = Q*R   R upper triangle and Q othogonal
template <typename Matrix>
std::pair<typename mtl::matrix::dense2D<typename Collection<Matrix>::value_type, matrix::parameters<> >,
	  typename mtl::matrix::dense2D<typename Collection<Matrix>::value_type, matrix::parameters<> > >
inline qr_factors(const Matrix& A)
{
	vampir_trace<4014> tracer;
    using std::abs;
    typedef typename Collection<Matrix>::value_type   value_type;
    // typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A);
    value_type       zero= math::zero(A[0][0]), one= math::one(A[0][0]);

    //evaluation of Q
    Matrix  Q(nrows, nrows), Qk(nrows, nrows), HEL(nrows, ncols), R(nrows, ncols), R_tmp(nrows, ncols);
    Q= one; R= zero; HEL= zero;

    boost::tie(Q, R_tmp)= qr(A);
    R= upper(R_tmp);

    return std::make_pair(Q,R);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_QR_INCLUDE


#ifndef HAMILTONIANS_NONINTEGRABLE_HPP
#define HAMILTONIANS_NONINTEGRABLE_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class NonIntegrable
{
private:
	int n_;
	double J_;
	double h_;
public:

	NonIntegrable(int n, double J, double h)
		: n_(n), J_(J), h_(h)
	{
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "TFIsing"},
			{"n", n_},
			{"J", J_},
			{"h", h_}
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			s += -J_*smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += -1.0*smp.ratio(i);
			s += -h_*smp.sigmatAt(i);
		}
		return s;
	}

	Eigen::VectorXd getCol(long long int col) const
	{
		Eigen::VectorXd res = Eigen::VectorXd::Zero(1<<n_);
		for(int i = 0; i < n_; i++)
		{
			int s1 = (col >> i) & 1;
			int s2 = (col >> ((i+1) % n_)) & 1;
			long long int x = (1 << i);
			res(col ^ x) += -h_;
			res(col) += -J_*(1-2*s1)*(1-2*s2);
			res(col) += -1.0*(1-2*s1);
		}
		return res;
	}
};
#endif//HAMILTONIANS_NONINTEGRABLE_HPP


//  Boost integer/static_log2.hpp header file  -------------------------------//

//  (C) Copyright Daryle Walker 2001.  Permission to copy, use, modify, sell and

//  distribute this software is granted provided this copyright notice appears

//  in all copies.  This software is provided "as is" without express or

//  implied warranty, and with no claim as to its suitability for any purpose.

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_INTEGER_STATIC_LOG2_HPP

#define BOOST_INTEGER_STATIC_LOG2_HPP

#include <boost/integer_fwd.hpp>  // self include

#include <boost/config.hpp>  // for BOOST_STATIC_CONSTANT, etc.

#include <boost/limits.hpp>  // for std::numeric_limits

#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

#include <boost/pending/ct_if.hpp>  // for boost::ct_if<>

#endif

namespace boost

{

//  Implementation details  --------------------------------------------------//

namespace detail

{

// Forward declarations

template < unsigned long Val, int Place = 0, int Index

 = std::numeric_limits<unsigned long>::digits >

    struct static_log2_helper_t;

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

template < unsigned long Val, int Place >

    struct static_log2_helper_t< Val, Place, 1 >;

#else

template < int Place >

    struct static_log2_helper_final_step;

template < unsigned long Val, int Place = 0, int Index

 = std::numeric_limits<unsigned long>::digits >

    struct static_log2_helper_nopts_t;

#endif

// Recursively build the logarithm by examining the upper bits

template < unsigned long Val, int Place, int Index >

struct static_log2_helper_t

{

private:

    BOOST_STATIC_CONSTANT( int, half_place = Index / 2 );

    BOOST_STATIC_CONSTANT( unsigned long, lower_mask = (1ul << half_place)

     - 1ul );

    BOOST_STATIC_CONSTANT( unsigned long, upper_mask = ~lower_mask );

    BOOST_STATIC_CONSTANT( bool, do_shift = (Val & upper_mask) != 0ul );

    BOOST_STATIC_CONSTANT( unsigned long, new_val = do_shift ? (Val

     >> half_place) : Val );

    BOOST_STATIC_CONSTANT( int, new_place = do_shift ? (Place + half_place)

     : Place );

    BOOST_STATIC_CONSTANT( int, new_index = Index - half_place );

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

    typedef static_log2_helper_t<new_val, new_place, new_index>  next_step_type;

#else

    typedef static_log2_helper_nopts_t<new_val, new_place, new_index>  next_step_type;

#endif

public:

    BOOST_STATIC_CONSTANT( int, value = next_step_type::value );

};  // boost::detail::static_log2_helper_t

// Non-recursive case

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

template < unsigned long Val, int Place >

struct static_log2_helper_t< Val, Place, 1 >

{

public:

    BOOST_STATIC_CONSTANT( int, value = Place );

};  // boost::detail::static_log2_helper_t

#else

template < int Place >

struct static_log2_helper_final_step

{

public:

    BOOST_STATIC_CONSTANT( int, value = Place );

};  // boost::detail::static_log2_helper_final_step

template < unsigned long Val, int Place, int Index >

struct static_log2_helper_nopts_t

{

private:

    typedef static_log2_helper_t<Val, Place, Index>  recursive_step_type;

    typedef static_log2_helper_final_step<Place>     final_step_type;

    typedef typename ct_if<( Index != 1 ), recursive_step_type,

     final_step_type>::type  next_step_type;

public:

    BOOST_STATIC_CONSTANT( int, value = next_step_type::value );

};  // boost::detail::static_log2_helper_nopts_t

#endif

}  // namespace detail

//  Compile-time log-base-2 evaluator class declaration  ---------------------//

template < unsigned long Value >

struct static_log2

{

    BOOST_STATIC_CONSTANT( int, value

     = detail::static_log2_helper_t<Value>::value );

};

template < >

struct static_log2< 0ul >

{

    // The logarithm of zero is undefined.

};

}  // namespace boost

#endif  // BOOST_INTEGER_STATIC_LOG2_HPP


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

#include "nlmatode.h"

int main() {
  double T = 1;
  unsigned int n = 3;

  Eigen::MatrixXd Y0(n, n);
  Y0 << 1, 1, 0, 0, 3, 2, 1, 5, 2;

  Eigen::MatrixXd YT = NLMatODE::matode(Y0, T);

  std::cout << YT << std::endl << std::endl;
  Eigen::MatrixXd M0(n, n);
  M0 << 1, 1, 0, 1, 3, 1, 0, 1, 1;
  // check whether invariant is perserved or not
  bool is_invariant = NLMatODE::checkinvariant(Y0, T);
  bool is_invar_sym = NLMatODE::checkinvariant(M0, T);
  std::cout << "Test whether invariant was preserved or not..." << std::endl;

  if (is_invariant) {
    std::cout << "Invariant for Y0 preserved." << std::endl;
  } else {
    std::cout << "Invariant for Y0 NOT preserved." << std::endl;
  }
  if (is_invar_sym) {
    std::cout << "Invariant for M0 preserved." << std::endl;
  } else {
    std::cout << "Invariant for M0 NOT preserved." << std::endl;
  }

  double rate = NLMatODE::cvgDiscreteGradientMethod();
  std::cout << "\nThe fitted rate for the discrete gradient method is:\n"
            << rate << std::endl;
  return 0;
}


#include "cluster_utils.h"

#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include "lib/progressbar/progressbar.h"
#include "proto_utils.h"

Eigen::MatrixXd bayesmix::posterior_similarity(
    const Eigen::MatrixXd &alloc_chain) {
  unsigned int n_data = alloc_chain.cols();
  Eigen::MatrixXd mean_diss = Eigen::MatrixXd::Zero(n_data, n_data);
  // Loop over pairs (i,j) of data points
  for (int i = 0; i < n_data; i++) {
    for (int j = 0; j < i; j++) {
      Eigen::ArrayXd diff = alloc_chain.col(i) - alloc_chain.col(j);
      mean_diss(i, j) = (diff == 0).count();
    }
  }
  return mean_diss / alloc_chain.rows();
}

Eigen::VectorXd bayesmix::cluster_estimate(
    const Eigen::MatrixXd &alloc_chain) {
  // Initialize objects
  unsigned n_iter = alloc_chain.rows();
  unsigned int n_data = alloc_chain.cols();
  std::vector<Eigen::SparseMatrix<double> > all_diss;
  progresscpp::ProgressBar bar(n_iter, 60);

  // Compute mean
  std::cout << "(Computing mean dissimilarity... " << std::flush;
  Eigen::MatrixXd mean_diss = bayesmix::posterior_similarity(alloc_chain);
  std::cout << "Done)" << std::endl;

  // Compute Frobenius norm error of all iterations
  Eigen::VectorXd errors(n_iter);
  for (int k = 0; k < n_iter; k++) {
    for (int i = 0; i < n_data; i++) {
      for (int j = 0; j < i; j++) {
        int x = (alloc_chain(k, i) == alloc_chain(k, j));
        errors(k) += (x - mean_diss(i, j)) * (x - mean_diss(i, j));
      }
    }
    // Progress bar
    ++bar;
    bar.display();
  }
  bar.done();

  // Find iteration with the least error
  std::ptrdiff_t ibest;
  unsigned int min_err = errors.minCoeff(&ibest);
  return alloc_chain.row(ibest).transpose();
}


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

/*
  Copyright (C) 2018 Klaus Spanderen

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

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

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

#include "fdcev.hpp"
#include "utilities.hpp"

#include <ql/math/functional.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/statistics/generalstatistics.hpp>
#include <ql/pricingengines/vanilla/analyticcevengine.hpp>
#include <ql/pricingengines/vanilla/fdcevvanillaengine.hpp>

#include <ql/methods/finitedifferences/utilities/cevrndcalculator.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;
using boost::unit_test_framework::test_suite;

namespace {
    class ExpectationFct {
      public:
        ExpectationFct(const CEVRNDCalculator& calculator, Time t)
        : t_(t), calculator_(calculator) { }

        Real operator()(Real f) const { return f*calculator_.pdf(f, t_); }

      private:
        const Time t_;
        const CEVRNDCalculator& calculator_;
    };
}

void FdCevTest::testLocalMartingale() {
    BOOST_TEST_MESSAGE(
        "Testing local martingale property of CEV process with PDF ...");

    const Time t = 1.0;

    const Real f0 = 2.1;
    const Real alpha = 1.75;
    const Real betas[] = {-2.4, 0.23, 0.9, 1.1, 1.5};

    for (Size i=0; i < LENGTH(betas); ++i) {
        const Real beta = betas[i];
        const CEVRNDCalculator rndCalculator(f0, alpha, beta);

        const Real eps = 1e-10;
        const Real tol = 100*eps;

        const Real upperBound = 10*rndCalculator.invcdf(1-eps, t);

        const Real expectationValue = GaussLobattoIntegral(10000, eps)(
            ExpectationFct(rndCalculator, t), QL_EPSILON, upperBound);

        const Real diff = expectationValue-f0;

        if (beta < 1.0 && std::fabs(diff) > tol) {
            BOOST_ERROR("CEV process should be a martingale for beta < 1.0"
                        << "\n    expected:   " << f0
                        << std::scientific
                        << "\n    difference  " << diff
                        << "\n    tolerance:  " << tol);
        }

        if (beta > 1.0 && diff > -tol) {
            BOOST_ERROR("CEV process should only be a local martingale "
                        "for beta > 1.0. Expectation is E[F_t|F_0] < F_0"
                        << "\n    E[F_t|F_0]: " << expectationValue
                        << "\n    F_0:        " << f0);
        }

        // check local martingale property with Monte-Carlo simulation
        const Size nSims = 5000;

        const Size nSteps = 2000;
        const Real dt = t / nSteps;
        const Real sqrtDt = std::sqrt(dt);

        GeneralStatistics stat;
        const PseudoRandom::rng_type mt(MersenneTwisterUniformRng(42));

        if (beta > 1.2) {
            for (Size i=0; i < nSims; ++i) {
                Real f = f0;
                for (Size j=0; j < nSteps; ++j) {
                    f += alpha * std::pow(f, beta) * mt.next().value * sqrtDt;
                    f = std::max(0.0, f);

                    if (f == 0.0) break; // absorbing boundary
                }
                stat.add(f - f0);
            }

            const Real calculated = stat.mean();
            const Real error = stat.errorEstimate();

            if (std::fabs(calculated - diff) > 2.35*error) {
                BOOST_ERROR(
                    "failed to calculate local martingale property "
                    "by Monte-Carlo Simulation for beta > 1.0. "
                            << "\n    E[F_t|F_0]   : " << expectationValue
                            << "\n    E_MC[F_t|F_0]: " << calculated + f0
                            << "\n    error_MC     : " << error
                            << "\n    difference   : " << std::fabs(calculated - diff)
                            << "\n    tolerance    : " << 2.35*error);
            }
        }
    }
}

void FdCevTest::testFdmCevOp() {
    BOOST_TEST_MESSAGE(
            "Testing FDM constant elasticity of variance (CEV) operator...");

    SavedSettings backup;

    const Date today = Date(22, February, 2018);
    const DayCounter dc = Actual365Fixed();
    Settings::instance().evaluationDate() = today;

    const Date maturityDate = today + Period(12, Months);
    const Real strike = 2.3;

    const Option::Type optionTypes[] = { Option::Call, Option::Put};

    const ext::shared_ptr<Exercise> exercise =
        ext::make_shared<EuropeanExercise>(maturityDate);

    for (Size i=0; i < LENGTH(optionTypes); ++i) {
        const Option::Type optionType = optionTypes[i];

        const ext::shared_ptr<PlainVanillaPayoff> payoff =
            ext::make_shared<PlainVanillaPayoff>(optionType, strike);

        const ext::shared_ptr<YieldTermStructure> rTS =
            flatRate(today, 0.15, dc);

        const Real f0 = 2.1;
        const Real alpha = 0.75;

        const Real betas[] = { -2.0, -0.5, 0.45, 0.6, 0.9, 1.45 };
        for (Size j=0; j < LENGTH(betas); ++j) {

            const Real beta = betas[j];

            VanillaOption option(payoff, exercise);
            option.setPricingEngine(ext::make_shared<AnalyticCEVEngine>(
                f0, alpha, beta, Handle<YieldTermStructure>(rTS)));

            const Real analyticNPV = option.NPV();

            const Real eps = 1e-3;

            option.setPricingEngine(ext::make_shared<AnalyticCEVEngine>(
                f0*(1+eps), alpha, beta, Handle<YieldTermStructure>(rTS)));
            const Real analyticUpNPV = option.NPV();

            option.setPricingEngine(ext::make_shared<AnalyticCEVEngine>(
                f0*(1-eps), alpha, beta, Handle<YieldTermStructure>(rTS)));
            const Real analyticDownNPV = option.NPV();

            const Real analyticDelta = (analyticUpNPV - analyticDownNPV)
                /(2*eps*f0);

            option.setPricingEngine(ext::make_shared<FdCEVVanillaEngine>(
                f0, alpha, beta, Handle<YieldTermStructure>(rTS),
                100, 1000, 1, 1.0, 1e-6));

            const Real calculatedNPV = option.NPV();
            const Real calculatedDelta = option.delta();

            const Real tol = 0.01;
            if (std::fabs(calculatedNPV - analyticNPV) > tol
                || std::fabs(calculatedDelta - analyticDelta) > tol) {
                BOOST_ERROR(
                    "failed to calculate vanilla option prices/delta "
                    << "\n    beta            : " << beta
                    << "\n    option type     : "
                    << ((payoff->optionType() == Option::Call) ? "Call" : "Put")
                    << "\n    analytic npv    : " << analyticNPV
                    << "\n    pde npv         : " << calculatedNPV
                    << "\n    npv difference  : "
                    << std::fabs(calculatedNPV - analyticNPV)
                    << "\n    tolerance       : " << tol
                    << "\n    analytic delta  : " << analyticDelta
                    << "\n    pde delta       : " << calculatedDelta
                    << "\n    delta difference: "
                    << std::fabs(calculatedDelta - analyticDelta)
                    << "\n    tolerance       : " << tol);
            }
        }
    }
}

test_suite* FdCevTest::suite(SpeedLevel speed) {
    test_suite* suite = BOOST_TEST_SUITE("Finite Difference CEV tests");

    suite->add(QUANTLIB_TEST_CASE(&FdCevTest::testLocalMartingale));
    suite->add(QUANTLIB_TEST_CASE(&FdCevTest::testFdmCevOp));

    return suite;
}


//
//  soptolsTestQuadratic.cpp
//
//
//  Created by Jose V. Alcala Burgos on 7/30/13.
//  Copyright [2013] Jose V. Alcala Burgos
//

#include "base/soptolsTestQuadratic.h"

// System
#include <armadillo>

#include <cmath>
#include <ctime>
#include <iostream>
#include <random>

// Project
#include "base/seqols.h"
#include "base/soptols.h"
#include "base/stochasticTools.h"

using arma::mat;

int main(int argc, char * const argv[]) {
  // Initialize the random number generator
  mt19937 generator;

  // Simulation parameters
  int num_parameters = 4;
  int num_initial_samples = num_parameters + 1;  // Ensure precision full rank

  // Inititalize the quadratic test parameters randomly.

  // The Hessian matrix for the quadratic loss.
  mat H = randu<mat>(num_parameters, num_parameters);
  H = H * H.t();  // Hessian is symmetric and positive definite

  // The covariance matrix for the quadratic loss.
  mat Sigma = randu<mat>(num_parameters, num_parameters);
  Sigma = Sigma * Sigma.t();

  // The parameter we will try to estimate.
  mat alpha_optimal = randu<mat>(1, num_parameters);

  // Print to screen the parameters created randomly.
  H.print(" H : ");
  mat G = H.i();
  G.print(" G : ");
  Sigma.print(" Sigma : ");
  alpha_optimal.print(" alpha_optimal : ");

  // Construct the stochastic gradient sampler
  stochasticGradient stoGrad(H, Sigma, alpha_optimal);

  // Create the first num_initial_samples + 1  samples randomly
  // The matrix with num_initial_samples + 1 initial predictors
  mat X = randu<mat>(num_initial_samples + 1, num_parameters);

  // Save parameters to csv files
  H.save("output/H.mat", csv_ascii);
  Sigma.save("output/Sigma.mat", csv_ascii);
  alpha_optimal.save("output/alpha_optimal.mat", csv_ascii);
  X.save("output/X.mat", csv_ascii);

  // Construct the Stochastic Optimization
  SOptOls sopt(stoGrad, X);
  sopt.optimize(generator);

  return 0;
}


/**
 * \file      probability-law-simulation.hpp
 * \author    Mehdi Benallegue
 * \date       2013
 * \brief
 *
 *
 *
 */

#ifndef SENSORSSIMULATIONPROBABILITYLAWSIMULATIONHPP
#define SENSORSSIMULATIONPROBABILITYLAWSIMULATIONHPP

#include <boost/random.hpp>

#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>

namespace stateObservation
{
namespace tools
{
class STATE_OBSERVATION_DLLAPI ProbabilityLawSimulation
{
public:
  /// gets a scalar Gaussian random variable
  /// having a given bias and standard deviation(std)
  /// default is the cetered unit Gaussian
  double getGaussianScalar(double std = 1, double bias = 0);

  /// gets vector Gaussian random variable
  /// having a given bias and standard deviation(std)
  static Matrix getGaussianVector(const Matrix & std, const Matrix & bias, Index rows, Index cols = 1);

protected:
  static boost::lagged_fibonacci1279 gen_;
};

} // namespace tools
} // namespace stateObservation

#endif // SENSORSSIMULATIONPROBABILITYLAWSIMULATIONHPP


#pragma once

#include <tf/tf.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// std
#include <iostream>

#include "darkroom/Triangulation.hpp"

using namespace Eigen;
using namespace std;

namespace PoseEstimatorSensorDistance {

// Generic functor for Eigen Levenberg-Marquardt minimizer
    template<typename _Scalar, int NX = Dynamic, int NY = Dynamic>
    struct Functor {
        typedef _Scalar Scalar;
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Matrix<Scalar, InputsAtCompileTime, 1> InputType;
        typedef Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
        typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

        const int m_inputs, m_values;

        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }

        int values() const { return m_values; }
    };

    struct PoseEstimator : Functor<double> {
        /**
         * Default amount of sensors needed for Eigen templated structure
         * @param numberOfSamples the pose will be estimated using this amount of samples
         * @param distanceBetweenSensors the distance between the two sensors in mm
         */
        PoseEstimator(int numberOfSamples, double distanceBetweenSensors, MatrixXd &rays0_A, MatrixXd &rays0_B,
                      MatrixXd &rays1_A, MatrixXd &rays1_B);

        /**
         * This is the function that is called in each iteration
         * @param x the pose vector (3 rotational 3 translational parameters)
         * @param fvec the error function (the difference between the sensor positions)
         * @return
         */
        int operator()(const VectorXd &x, VectorXd &fvec) const;

        VectorXd pose;
        MatrixXd rays0_A, rays0_B, rays1_A, rays1_B; // rays of each lighthouse pointing to the sensors
        double distanceBetweenSensors;
        int numberOfSamples;
        Matrix4d RT_A, RT_B;
        static int counter;
    };

}


#include "DenoiseSystem.h"

#include "../Components/DenoiseData.h"

#include <_deps/imgui/imgui.h>

#include <spdlog/spdlog.h>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Dense>

#include <vector>
#include <deque>
#include <set>

using namespace Eigen;
using namespace Ubpa;

using vertpair = std::pair<int, int>;

enum collapse_method {
	COLLAPSE_TO_V1,
	COLLAPSE_TO_V2,
	COLLAPSE_TO_MEAN
};

inline Vector4f homogenous(const valf3& vec) {
	return Vector4f(vec[0], vec[1], vec[2], 1);
}

float error(const Vector4f& homo, const Matrix4f& Q) {
	return abs((float)(homo.transpose() * Q * homo));
}

class Contr {
public:
	vertpair vp;
	Matrix4f Q;
	Vector4f loc;
	collapse_method method;
	float resultError;

	Contr(
		const vertpair& vertp, const std::vector<Vertex*>& verteces,
		const std::vector<Matrix4f>& initialQ) {
		vp = vertp;
		findMinError(verteces, initialQ);
	}

	void findMinError(const std::vector<Vertex*>& verteces,
		const std::vector<Matrix4f>& initialQ) {
		Vector4f loc_v1 = homogenous(verteces[vp.first]->position);
		Vector4f loc_v2 = homogenous(verteces[vp.second]->position);
		Vector4f loc_vm = (loc_v1 + loc_v2) / 2;

		Q = initialQ[vp.first] + initialQ[vp.second];

		float err_v1 = error(loc_v1, Q),
			err_v2 = error(loc_v2, Q),
			err_vm = error(loc_vm, Q);

		if (err_v1 < err_v2) {
			if (err_vm < err_v1) method = COLLAPSE_TO_MEAN;
			else method = COLLAPSE_TO_V1;
		}
		else {
			if (err_vm < err_v2) method = COLLAPSE_TO_MEAN;
			else method = COLLAPSE_TO_V2;
		}

		if (method == COLLAPSE_TO_V1) {
			loc = loc_v1;
			resultError = err_v1;
		}
		else if (method == COLLAPSE_TO_V2) {
			loc = loc_v2;
			resultError = err_v2;
		}
		else {
			loc = loc_vm;
			resultError = err_vm;
		}
	}

	bool operator<(const Contr& other) const {
		return resultError < other.resultError;
	}

	bool contains(int vid) {
		return vid == vp.first || vid == vp.second;
	}

	void perform(std::vector<Vertex*>& verteces, std::vector<Matrix4f>& initialQ,
		std::vector<std::vector<Triangle*> >& vertexToFaces,
		std::set<int>& facesToRemove,
		std::set<int>& verticesToRemove,
		std::deque<Contr>& edges,
		std::set<vertpair>& existingedges,
		std::vector<std::vector<Vertex*> >&
		faceAdjVert) {

		initialQ[vp.first] = Q;

		int keep = vp.first, remove = vp.second;
		if (method == COLLAPSE_TO_MEAN) {
			valf3 tmp = verteces[keep]->position;
			tmp += verteces[remove]->position;
			tmp /= 2;
			verteces[keep]->position = tmp;
		}
		else if (method == COLLAPSE_TO_V2) {
			verteces[keep]->position = verteces[remove]->position;
		}

		std::vector<Triangle*> faces = vertexToFaces[remove];
		for (int i = 0; i < faces.size(); i++) {
			Triangle* f = faces[i];

			int v1idx = -1, v2idx = -1;
			for (int j = 0; j < faceAdjVert[f->id].size(); j++) {
				if (faceAdjVert[f->id][j]->id == keep) v1idx = j;
				else if (faceAdjVert[f->id][j]->id == remove) v2idx = j;
			}

			verticesToRemove.insert(remove);
			faceAdjVert[f->id][v2idx] = verteces[keep];
			if (v1idx == -1) {
				vertexToFaces[keep].push_back(f);
			}
			else {
				facesToRemove.insert(f->id);
			}
		}

		std::deque<int> edgesToRemove;
		for (int i = 0; i < edges.size(); i++) {
			if (vp == edges[i].vp) continue;
			if (edges[i].contains(keep)) {
				edges[i].findMinError(verteces, initialQ);
			}
			else if (edges[i].contains(remove)) {
				vertpair possible;
				if (edges[i].vp.first == remove) {
					possible = std::make_pair(edges[i].vp.second, keep);
				}
				else {
					possible = std::make_pair(edges[i].vp.first, keep);
				}

				if (existingedges.find(possible) != existingedges.end()) {
					edgesToRemove.push_back(i);
				}
				else {
					edges[i].vp = possible;
					edges[i].findMinError(verteces, initialQ);
				}
			}
		}

		int offset = 0;
		for (int i = 0; i < edgesToRemove.size(); i++) {
			existingedges.erase(edges[edgesToRemove[i]].vp);
			edges.erase(edges.begin() + edgesToRemove[i] + offset);
			offset--;
		}

		vertexToFaces[remove].clear();
	}
};

Contr popmin(std::deque<Contr>& edges) {
	Contr best = edges.front();
	int bestidx = 0;
	for (int i = 1; i < edges.size(); i++) {
		if (edges[i] < best) {
			bestidx = i;
			best = edges[i];
		}
	}
	edges[bestidx] = edges.back();
	edges.pop_back();
	return best;
}

void DenoiseSystem::OnUpdate(Ubpa::UECS::Schedule& schedule) {
	schedule.RegisterCommand([](Ubpa::UECS::World* w) {
		auto data = w->entityMngr.GetSingleton<DenoiseData>();
		if (!data)
			return;

		if (ImGui::Begin("Denoise")) {
			if (ImGui::Button("Mesh to HEMesh")) {
				data->heMesh->Clear();
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					if (data->mesh->GetSubMeshes().size() != 1) {
						spdlog::warn("number of submeshes isn't 1");
						return;
					}

					data->copy = *data->mesh;

					std::vector<size_t> indices(data->mesh->GetIndices().begin(), data->mesh->GetIndices().end());

					data->heMesh->Init(indices, 3);
					if (!data->heMesh->IsTriMesh())
						spdlog::warn("HEMesh init fail");

					for (size_t i = 0; i < data->mesh->GetPositions().size(); i++) {
						data->heMesh->Vertices().at(i)->position = data->mesh->GetPositions().at(i);
						data->heMesh->Vertices().at(i)->id = -1;
					}

					spdlog::info("Mesh to HEMesh success");
				}();
			}

			if (ImGui::Button("QEM")) {
				[&]() {
					if (!data->heMesh->IsTriMesh()) {
						spdlog::warn("HEMesh isn't triangle mesh");
						return;
					}

					const auto vertices = data->heMesh->Vertices();
					auto total = vertices.size();
					for (int i = 0; i < total; ++i) vertices[i]->id = i;

					data->faces.clear();
					data->faces.resize(data->heMesh->Polygons().size() + 1, std::vector<Vertex*>());

					std::vector<Vertex*> verteces(total + 1, NULL);
					std::vector<Matrix4f> initialQ(total + 1, Matrix4f::Zero());
					std::vector<std::vector<Triangle*> > vertexToFaces(total + 1, std::vector<Triangle*>());

					std::set<vertpair> edgeset;

					for (int i = 0; i < data->heMesh->Polygons().size(); i++) {
						Triangle* f = data->heMesh->Polygons()[i];
						f->id = i;

						const auto& adj = f->AdjVertices();

						valf3 v1 = adj[0]->position;
						valf3 v2 = adj[1]->position;
						valf3 v3 = adj[2]->position;

						Vector4f p;
						/* http://paulbourke.net/geometry/planeeq/ */
						p[0] = v1[1] * (v2[2] - v3[2]) + v2[1] * (v3[2] - v1[2]) + v3[1] * (v1[2] - v2[2]);
						p[1] = v1[2] * (v2[0] - v3[0]) + v2[2] * (v3[0] - v1[0]) + v3[2] * (v1[0] - v2[0]);
						p[2] = v1[0] * (v2[1] - v3[1]) + v2[0] * (v3[1] - v1[1]) + v3[0] * (v1[1] - v2[1]);
						p[3] = -(v1[0] * (v2[1] * v3[2] - v3[1] * v2[2]) +
							v2[0] * (v3[1] * v1[2] - v1[1] * v3[2]) +
							v3[0] * (v1[1] * v2[2] - v2[1] * v1[2]));

						Matrix4f pp = p * p.transpose();

						for (auto vert : f->AdjVertices()) {
							verteces[vert->id] = vert;
							vertexToFaces[vert->id].push_back(f);
							data->faces[f->id].push_back(vert);

							initialQ[vert->id] += pp;
						}

						for (const auto& edge : f->AdjEdges()) {
							std::vector<Vertex*> tmpV;
							for (const auto& vert : edge->AdjVertices()) {
								tmpV.push_back(vert);
							}
							vertpair vp = vertpair(tmpV[0]->id, tmpV[1]->id);
							edgeset.insert(vp);
						}
					}

					std::deque<Contr> edges;
					for (auto edge = edgeset.begin(); edge != edgeset.end(); edge++) {
						edges.push_back(Contr(*edge, verteces, initialQ));
					}

					data->facesToRemove.clear();
					data->verticesToRemove.clear();

					int target_edges = (int)(data->scale * (float)edges.size());
					while (edges.size() > target_edges) {
						Contr best = popmin(edges);
						best.perform(verteces, initialQ, vertexToFaces, data->facesToRemove, data->verticesToRemove, edges, edgeset, data->faces);
					}
				}();
			}

			if (ImGui::Button("HEMesh to Mesh")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}

					if (!data->heMesh->IsTriMesh() || data->heMesh->IsEmpty()) {
						spdlog::warn("HEMesh isn't triangle mesh or is empty");
						return;
					}

					data->mesh->SetToEditable();

					const size_t N = data->heMesh->Vertices().size();
					const size_t M = data->heMesh->Polygons().size();
					std::vector<Ubpa::pointf3> positions;
					std::vector<uint32_t> indices;
					std::vector<int> remapping(N + 1, -1);
					for (size_t i = 0; i < N; i++) {
						int id = data->heMesh->Vertices().at(i)->id;
						//if (data->verticesToRemove.find(id) == data->verticesToRemove.end()) {
							positions.push_back(data->heMesh->Vertices().at(i)->position);
							remapping[id] = static_cast<int>(positions.size() - 1);
						//}
					}
					for (size_t i = 0; i < M; i++) {
						int id = i;
						if (data->facesToRemove.find(id) == data->facesToRemove.end()) {
							/*if (remapping[data->faces[id][0]->id] == -1 ||
								remapping[data->faces[id][1]->id] == -1 ||
								remapping[data->faces[id][2]->id] == -1) continue;*/
							indices.push_back(static_cast<uint32_t>(remapping[data->faces[id][0]->id]));
							indices.push_back(static_cast<uint32_t>(remapping[data->faces[id][1]->id]));
							indices.push_back(static_cast<uint32_t>(remapping[data->faces[id][2]->id]));
						}
					}
					const size_t M3 = indices.size();
					data->mesh->SetColors({});
					data->mesh->SetUV({});
					data->mesh->SetNormals({});
					data->mesh->SetPositions(std::move(positions));
					data->mesh->SetIndices(std::move(indices));
					data->mesh->SetSubMeshCount(1);
					data->mesh->SetSubMesh(0, { 0, M3 });
					data->mesh->GenUV();
					data->mesh->GenNormals();
					data->mesh->GenTangents();

					spdlog::info("HEMesh to Mesh success");
				}();
			}

			if (ImGui::Button("Recover Mesh")) {
				[&]() {
					if (!data->mesh) {
						spdlog::warn("mesh is nullptr");
						return;
					}
					if (data->copy.GetPositions().empty()) {
						spdlog::warn("copied mesh is empty");
						return;
					}

					*data->mesh = data->copy;

					spdlog::info("recover success");
				}();
			}
		}
		ImGui::End();
	});
}


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// 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 "../../blas/test/random.hpp"

#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/lapack/computational/geqrf.hpp>
#include <boost/numeric/bindings/lapack/computational/ormqr.hpp>
#include <boost/numeric/bindings/lapack/computational/orgqr.hpp>
#include <boost/numeric/bindings/lapack/computational/unmqr.hpp>
#include <boost/numeric/bindings/lapack/computational/ungqr.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/mpl/if.hpp>

#include <iostream>
#include <limits>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;
namespace tag = boost::numeric::bindings::tag;

// Randomize a matrix
template <typename M>
void randomize(M& m) {
   typedef typename M::size_type  size_type ;
   typedef typename M::value_type value_type ;

   size_type size1 = m.size1() ;
   size_type size2 = m.size2() ;

   for (size_type i=0; i<size2; ++i) {
      for (size_type j=0; j<size1; ++j) {
         m(j,i) = random_value< value_type >() ;
      }
   }
} // randomize()

template <typename M>
ublas::triangular_adaptor<const M, ublas::upper> upper_part(const M& m) {
   return ublas::triangular_adaptor<const M, ublas::upper>( m );
}

template <typename T, typename W>
int do_memory_type(int n, W workspace) {
   typedef typename bindings::remove_imaginary<T>::type real_type ;
   typedef std::complex< real_type >                                            complex_type ;

   typedef ublas::matrix<T, ublas::column_major> matrix_type ;
   typedef ublas::vector<T>                      vector_type ;

   // Set matrix
   matrix_type a( n, n );
   vector_type tau( n );

   randomize( a );
   matrix_type a2( a );
   matrix_type a3( a );

   // Compute QR factorization.
   lapack::geqrf( a, tau, workspace ) ;

   // Apply the orthogonal transformations to a2
   if( boost::is_complex<T>::value ) {
        lapack::unmqr( tag::left(), bindings::conj( a ), tau, a2, workspace );
   } else {
        lapack::unmqr( tag::left(), bindings::trans( a ), tau, a2, workspace );
   }

   // The upper triangular parts of a and a2 must be equal.
   if (norm_frobenius( upper_part( a - a2 ) )
            > std::numeric_limits<real_type>::epsilon() * 10.0 * norm_frobenius( upper_part( a ) ) ) return 255 ;

   // Generate orthogonal matrix
   lapack::ungqr( a, tau, workspace );

   // The result of lapack::ormqr and the equivalent matrix product must be equal.
   if (norm_frobenius( a2 - prod(herm(a), a3) )
            > std::numeric_limits<real_type>::epsilon() * 10.0 * norm_frobenius( a2 ) ) return 255 ;

   return 0 ;
} // do_value_type()

template <typename T>
int do_value_type() {
   const int n = 8 ;

   if (do_memory_type<T,lapack::optimal_workspace>( n, lapack::optimal_workspace() ) ) return 255 ;
   if (do_memory_type<T,lapack::minimal_workspace>( n, lapack::minimal_workspace() ) ) return 255 ;

   ublas::vector<T> work( n );
   if (do_memory_type<T, lapack::detail::workspace1<ublas::vector<T> > >( n, lapack::workspace(work) ) ) return 255 ;
   return 0;
} // do_value_type()

int main() {
   // Run tests for different value_types
   if (do_value_type<float>()) return 255;
   if (do_value_type<double>()) return 255;
   if (do_value_type< std::complex<float> >()) return 255;
   if (do_value_type< std::complex<double> >()) return 255;

   std::cout << "Regression test succeeded\n" ;
   return 0;
}


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include "recti/fractions.hpp"
// #include <boost/multiprecision/cpp_int.hpp>
#include <doctest/doctest.h>
#include <iostream>

using namespace fun;

TEST_CASE("GCD")
{
    CHECK(gcd(0, 0) == 0);
    CHECK(gcd(1, 0) == 1);
    CHECK(gcd(0, 1) == 1);
    CHECK(gcd(-1, 0) == 1);
    CHECK(gcd(0, -1) == 1);
    CHECK(lcm(0, 0) == 0);
    CHECK(lcm(1, 0) == 0);
    CHECK(lcm(0, 1) == 0);
}

TEST_CASE("Fraction")
{
    // using boost::multiprecision::cpp_int;
    // static_assert(Integral<cpp_int>);
    const auto a = 3;
    const auto b = 4;
    const auto c = 5;
    const auto d = 6;
    // const auto f = -30;
    // const auto g = 4;
    // const auto z = 0;
    // const auto h = -g;

    auto p = Fraction {a, b};
    std::cout << p << '\n';
    const auto q = Fraction {c, d};

    CHECK(p == Fraction(30, 40));
    CHECK(2 * p == Fraction(6, 4));
    p *= 2;
    CHECK(p == Fraction(6, 4));
    CHECK(p / 2 == Fraction(30, 40));

    p /= 2;
    CHECK(p + q == Fraction(19, 12));
    CHECK(p - q == Fraction(-1, 12));
    CHECK(p < q);
    CHECK(p != q);
    CHECK(0 < p);
}

TEST_CASE("Fraction Special Cases")
{
    const auto p = Fraction {3, 4};
    const auto inf = Fraction {1, 0};
    // const auto nan = Fraction {0, 0};
    const auto zero = Fraction {0, 1};

    CHECK(-inf < zero);
    CHECK(zero < inf);
    CHECK(-inf < p);
    CHECK(p < inf);
    CHECK(inf == inf);
    CHECK(-inf < inf);
    CHECK(inf == inf * p);
    CHECK(inf == inf * inf);
    CHECK(inf == p / zero);
    CHECK(inf == inf / zero);
    // CHECK(nan == nan);
    // CHECK(nan == inf * zero);
    // CHECK(nan == -inf * zero);
    // CHECK(nan == inf / inf);
    // CHECK(nan == nan * zero);
    // CHECK(nan == nan * nan);
    // CHECK(nan == inf - inf);
    CHECK(inf == inf + inf);
    CHECK(inf + p == inf);   // ???
    CHECK(-inf + p == -inf); // ???
    CHECK(p + zero == p);
}


/***************************************************************************
 *
 *   Copyright (C) 2021 by fxzjshm
 *   Licensed under the GNU General Public License, version 2.0
 *
 ***************************************************************************/

// pad missing channels with 0
// used for converting filterbank file to wave

#include <boost/program_options.hpp>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <numeric>

#include "io.hpp"
#include "types.h"

// Edited from https://www.geeksforgeeks.org/program-find-gcd-floating-point-numbers/
template <typename T, typename U, typename V>
T gcd(T a, U b, V epsilon) {
    if (a < b) {
        std::swap(a, b);
    }

    if (std::abs(b) < epsilon) {
        return a;
    }
    return (gcd(b, a - std::floor(a / b) * b, epsilon));
}

int main(int argc, char **argv) {
    boost::program_options::options_description all_option("Options");
    using boost::program_options::value;
    /* clang-format off */
    all_option.add_options()
        ("help,h", "Show help message")
        ("samples_count", value<size_t>()->required(), "Number of samples in file")
        ("input_file,f", value<std::string>()->required(), "Input file")
        ("output_file,o", value<std::string>()->required(), "Output file")
        ("in_text", "Read input file as text")
        ("out_text", "Write output file as text")
        ("fmax", value<float>()->required(), "Max of frequency of input channel")
        ("df", value<float>()->required(), "Input Channel bandwidth")
        ("nchans", value<size_t>()->required(), "Number of channels in input file")
        ("epsilon", value<float>()->required(), "Minial channel bandwidth in output file")
        ("fmax_out", value<float>()->required(), "Max of frequency of out channel")
        ("pad_value_real,pad_value", value<float>()->default_value(0.0f), "Value to fill real part in padded area")
        ("pad_imaginary_part", "Whether to pad imaginary part")
        ("pad_value_imaginary", value<float>()->default_value(0.0f), "Value to fill real part in padded area")
    ;
    /* clang-format on */
    boost::program_options::positional_options_description p;
    p.add("input_file", 1);
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(all_option).positional(p).run(), vm);
    boost::program_options::notify(vm);

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

    float in_fmax = vm["fmax"].as<float>();
    float in_df = std::abs(vm["df"].as<float>());
    float gcd_epsilon = vm["epsilon"].as<float>();
    float out_df = gcd(in_fmax, in_df, gcd_epsilon);
    size_t nchans = vm["nchans"].as<size_t>();
    float in_fmin = in_fmax - in_df * (nchans - 1);
    size_t samples_count = vm["samples_count"].as<size_t>();
    float out_fmax;
    if (vm.count("fmax_out")) {
        out_fmax = vm["fmax_out"].as<float>();
    } else {
        out_fmax = in_fmax;
    }
    if (out_fmax < in_fmax) {
        std::cerr << "Warn: setting out_fmax to in_max" << std::endl;
    }
    size_t out_seg_length = static_cast<size_t>(std::round(out_fmax / out_df)) + 1;
    float pad_value_real = vm["pad_value_real"].as<float>();

    std::cout << "out_df = " << out_df << std::endl
              << "out_seg_length = " << out_seg_length << std::endl;

    std::vector<data_type> h_in;
    std::string in_file_name = vm["input_file"].as<std::string>();
    std::string out_file_name = vm["output_file"].as<std::string>();
    size_t in_file_nsamps;
    if (vm.count("in_text")) {
        std::ifstream in_file_stream(in_file_name);
        // h_in = std::vector<data_type>(std::istream_iterator<data_type>(in_file_stream), {});
        data_type tmp;
        while (in_file_stream >> tmp) {
            h_in.push_back(tmp);
        }
        in_file_nsamps = h_in.size();
    } else {
        FILE *in_file_stream;
        in_file_stream = fopen(in_file_name.c_str(), "rb");
        size_t in_file_length = std::filesystem::file_size(in_file_name);
        in_file_nsamps = in_file_length / sizeof(data_type);
        h_in.resize(in_file_nsamps);
        fread(&h_in[0], 1, in_file_length, in_file_stream);
        fclose(in_file_stream);
    }
    write_vector(h_in, "pad_fil-h_in.txt");

    std::vector<data_type> h_out_real; // output is padded filterbank
    std::fill(h_out_real.begin(), h_out_real.end(), pad_value_real);
    h_out_real.resize(out_seg_length * samples_count, pad_value_real);

    for (size_t s = 0; s < samples_count; s++) {
        for (size_t i = 0; i < nchans; i++) {
            size_t in_idx = nchans * s + i;
            float f_current = in_fmax - in_df * i;
            size_t f_current_idx = static_cast<size_t>(std::round(f_current / out_df));
            assert(f_current_idx < out_seg_length);
            size_t out_idx = out_seg_length * s + f_current_idx;
            if (in_idx < h_in.size() && out_idx < h_out_real.size()) {
                h_out_real[out_idx] = h_in[in_idx];
            } else {
                std::cout << "Warning: "
                          << "in_idx = " << in_idx << ", "
                          << "h_in.size() = " << h_in.size() << ", "
                          << "out_idx = " << out_idx << ", "
                          << "h_out_real.size() = " << h_out_real.size() << std::endl;
            }
        }
    }

    if (vm.count("pad_imaginary_part")) {
        float pad_value_imaginary = vm["pad_value_imaginary"].as<float>();
        std::vector<data_type> h_out_complex(2 * h_out_real.size());
        for (size_t i = 0; i < h_out_real.size(); i++) {
            h_out_complex[2 * i] = h_out_real[i];
            h_out_complex[2 * i + 1] = pad_value_imaginary;
        }
        if (vm.count("out_text")) {
            write_vector(h_out_complex, 2 * out_seg_length, samples_count, out_file_name);
        } else {
            write_vector_binary(h_out_complex, 2 * out_seg_length * samples_count, out_file_name);
        }
    } else {
        if (vm.count("out_text")) {
            write_vector(h_out_real, out_seg_length, samples_count, out_file_name);
        } else {
            write_vector_binary(h_out_real, out_seg_length * samples_count, out_file_name);
        }
    }

    return 0;
}