/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120926    E. Dekens         File created.
 *      121218    S. Billemont      Added output fuctions to display Legendre polynomial data,
 *                                  for debugging.
 *
 *    References
 *
 *    Notes
 *
 */

#include <sstream>
#include <stdexcept>

#include <boost/exception/all.hpp>
#include <boost/math/special_functions/factorials.hpp>

#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{
namespace basic_mathematics
{

//! Default constructor, initializes cache object with 0 maximum degree and order.
LegendreCache::LegendreCache( const bool useGeodesyNormalization )
{
    useGeodesyNormalization_  = useGeodesyNormalization;

    if( useGeodesyNormalization_ )
    {
        legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction;
    }
    else
    {
        legendrePolynomialFunction_ = regularLegendrePolynomialFunction;
    }

    resetMaximumDegreeAndOrder( 0, 0 );

}

//! Constructor
LegendreCache::LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization  )
{
    useGeodesyNormalization_  = useGeodesyNormalization;

    if( useGeodesyNormalization_ )
    {
        legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction;
    }
    else
    {
        legendrePolynomialFunction_ = regularLegendrePolynomialFunction;
    }

    resetMaximumDegreeAndOrder( maximumDegree, maximumOrder );
}

//! Get Legendre polynomial from cache when possible, and from direct computation otherwise.
void LegendreCache::update( const double polynomialParameter  )
{
    currentPolynomialParameter_ = polynomialParameter;
    currentPolynomialParameterComplement_ = std::sqrt( 1.0 - polynomialParameter * polynomialParameter ); // cosine of latitude is always positive!

    LegendreCache& thisReference = *this;
    for( int i = 0; i <= maximumDegree_; i++ )
    {
        for( int j = 0; ( ( j <= i ) && ( j <= maximumOrder_ ) ) ; j++ )
        {
            legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] = legendrePolynomialFunction_( i, j, thisReference );
        }
    }
}

//! Update maximum degree and order of cache
void LegendreCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder )
{
    maximumDegree_ = maximumDegree;
    maximumOrder_ = maximumOrder;
    legendreValues_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) );

    currentPolynomialParameter_ = TUDAT_NAN;
    currentPolynomialParameterComplement_ = TUDAT_NAN;
}

//! Get Legendre polynomial value from the cache.
double LegendreCache::getLegendrePolynomial(
        const int degree, const int order )
{
    if( degree > maximumDegree_ || order > maximumOrder_ )
    {
        std::cerr<<"Error when requesting legendre cache, maximum degree or order exceeded "<<
                   degree<<" "<<maximumDegree_<<" "<<order<<" "<<maximumOrder_<<std::endl;
        return TUDAT_NAN;
    }
    else if( order > degree )
    {
        return 0.0;
    }
    else
    {
        return legendreValues_[ degree * ( maximumOrder_ + 1  ) + order ];
    };
}

//! Compute unnormalized associated Legendre polynomial.
double computeLegendrePolynomial( const int degree,
                                  const int order,
                                  LegendreCache& legendreCache )
{
    if( legendreCache.getUseGeodesyNormalization( ) )
    {
        throw std::runtime_error( "Error when computing Legendre polynomial, input uses normalization" );
    }

    // If degree or order is negative...
    if ( degree < 0 || order < 0 )
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage << "Error: the Legendre polynomial of = " << degree << " and order = "
                     << order << " is undefined." << std::endl;

        // Throw a run-time error.
        boost::throw_exception( boost::enable_error_info( std::runtime_error(
                                                              errorMessage.str( ) ) ) );
    }

    // Else if order is greater than degree...
    else if ( order > degree && degree >= 0 )
    {
        // Return zero.
        return 0.0;
    }

    // Else if order and degree are lower than 2...
    else if ( degree <= 1 && order <= 1 )
    {
        // Compute polynomial explicitly.
        return computeLegendrePolynomialExplicit( degree, order, legendreCache.getCurrentPolynomialParameter( ) );
    }

    // Else if degree and order are sectoral...
    else if ( degree == order )
    {
        // Obtain polynomial of degree one and order one.
        const double degreeOneOrderOnePolynomial = legendreCache.getLegendrePolynomial(
                    1, 1 );

        // Obtain prior sectoral polynomial.
        const double priorSectoralPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 1, order - 1 );

        // Compute polynomial.
        return computeLegendrePolynomialDiagonal(
                    degree, degreeOneOrderOnePolynomial, priorSectoralPolynomial );
    }

    // Else degree and order are zonal/tessoral...
    else
    {
        // Obtain prior degree polynomial.
        const double oneDegreePriorPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 1, order );

        // Obtain two degrees prior polynomial.
        const double twoDegreesPriorPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 2, order );

        // Compute polynomial.
        return computeLegendrePolynomialVertical( degree,
                                                  order,
                                                  legendreCache.getCurrentPolynomialParameter( ),
                                                  oneDegreePriorPolynomial,
                                                  twoDegreesPriorPolynomial );
    }
}

double computeLegendrePolynomial( const int degree,
                                  const int order,
                                  const double legendreParameter )
{
    LegendreCache legendreCache( degree, order, 0 );
    legendreCache.update( legendreParameter );
    return computeLegendrePolynomial( degree, order, legendreCache );
}

//! Compute geodesy-normalized associated Legendre polynomial.
double computeGeodesyLegendrePolynomial( const int degree,
                                         const int order,
                                         LegendreCache& geodesyLegendreCache )
{

    if( !geodesyLegendreCache.getUseGeodesyNormalization( ) )
    {
        throw std::runtime_error( "Error when computing Legendre polynomial, input uses no normalization" );
    }

    // If degree or order is negative...
    if ( degree < 0 || order < 0 )
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage << "Error: the Legendre polynomial of = " << degree << " and order = "
                     << order << " is undefined." << std::endl;

        // Throw a run-time error.
        boost::throw_exception( boost::enable_error_info( std::runtime_error(
                                                              errorMessage.str( ) ) ) );
    }

    // Else if order is greater than degree...
    else if ( order > degree && degree >= 0 )
    {
        // Return zero.
        return 0.0;
    }

    // Else if order and degree are lower than 2...
    else if ( degree <= 1 && order <= 1 )
    {
        // Compute polynomial explicitly.
        return computeGeodesyLegendrePolynomialExplicit( degree, order, geodesyLegendreCache.getCurrentPolynomialParameter( ) );
    }

    // Else if degree and order are sectoral...
    else if ( degree == order )
    {
        // Obtain polynomial of degree one and order one.
        double degreeOneOrderOnePolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    1, 1 );

        // Obtain prior sectoral polynomial.
        double priorSectoralPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 1, order - 1 );

        // Compute polynomial.
        return computeGeodesyLegendrePolynomialDiagonal(
                    degree, degreeOneOrderOnePolynomial, priorSectoralPolynomial );
    }

    // Else degree and order are zonal/tessoral...
    else
    {
        // Obtain prior degree polynomial.
        double oneDegreePriorPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 1, order );

        // Obtain two degrees prior polynomial.
        double twoDegreesPriorPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 2, order );

        // Compute polynomial.
        return computeGeodesyLegendrePolynomialVertical( degree,
                                                         order,
                                                         geodesyLegendreCache.getCurrentPolynomialParameter( ),
                                                         oneDegreePriorPolynomial,
                                                         twoDegreesPriorPolynomial );
    }
}

double computeGeodesyLegendrePolynomial( const int degree,
                                         const int order,
                                         const double legendreParameter )
{
    LegendreCache legendreCache( degree, order, 1 );
    legendreCache.update( legendreParameter );
    return computeGeodesyLegendrePolynomial( degree, order, legendreCache );
}

//! Compute derivative of unnormalized Legendre polynomial.
double computeLegendrePolynomialDerivative( const int order,
                                            const double polynomialParameter,
                                            const double currentLegendrePolynomial,
                                            const double incrementedLegendrePolynomial )
{
    // Return polynomial derivative.
    return incrementedLegendrePolynomial
            / std::sqrt( 1.0 - polynomialParameter * polynomialParameter )
            - static_cast< double >( order ) * polynomialParameter
            / ( 1.0 - polynomialParameter * polynomialParameter )
            * currentLegendrePolynomial;
}

//! Compute derivative of geodesy-normalized Legendre polynomial.
double computeGeodesyLegendrePolynomialDerivative( const int degree,
                                                   const int order,
                                                   const double polynomialParameter,
                                                   const double currentLegendrePolynomial,
                                                   const double incrementedLegendrePolynomial )
{
    // Compute normalization correction factor.
    double normalizationCorrection = std::sqrt( ( static_cast< double >( degree )
                                                  + static_cast< double >( order ) + 1.0 )
                                                * ( static_cast< double >( degree - order ) ) );

    // If order is zero apply multiplication factor.
    if ( order == 0 )
    {
        normalizationCorrection *= std::sqrt( 0.5 );
    }

    // Return polynomial derivative.
    return normalizationCorrection * incrementedLegendrePolynomial
            / std::sqrt( 1.0 - polynomialParameter * polynomialParameter )
            - static_cast< double >( order ) * polynomialParameter
            / ( 1.0 - polynomialParameter * polynomialParameter )
            * currentLegendrePolynomial;
}

//! Compute low degree/order unnormalized Legendre polynomial explicitly.
double computeLegendrePolynomialExplicit( const int degree,
                                          const int order,
                                          const double polynomialParameter )
{
    // Check which order is required for Legendre polynomial.
    switch( degree )
    {
    case 0:
        switch( order )
        {
        case 0:
            return 1.0;
        default:
            std::cerr << "Error, explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 1:
        switch( order )
        {
        case 0:
            return polynomialParameter;
        case 1:
            return std::sqrt( 1 - polynomialParameter * polynomialParameter );
        default:
            std::cerr << "Error, explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 2:
        switch( order )
        {
        case 0:
            return 0.5 * ( 3.0 * polynomialParameter * polynomialParameter - 1.0 );
        case 1:
            return 3.0 * polynomialParameter
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 3.0 * ( 1.0 - polynomialParameter * polynomialParameter );
        default:
            std::cerr << "Error, explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 3:
        switch( order )
        {
        case 0:
            return 0.5 * polynomialParameter
                    * ( 5.0 * polynomialParameter * polynomialParameter - 3.0 );
        case 1:
            return 1.5 * ( 5.0 * polynomialParameter * polynomialParameter - 1.0 )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 15.0 * polynomialParameter * ( 1.0 - polynomialParameter * polynomialParameter );
        case 3:
            return 15.0 * ( 1.0 - polynomialParameter * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        default:
            std::cerr << "Error, a explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 4:
        switch( order )
        {
        case 0:
            return ( 35.0 * polynomialParameter * polynomialParameter
                     * polynomialParameter * polynomialParameter
                     - 30.0 * polynomialParameter * polynomialParameter + 3.0 ) / 8.0;
        case 1:
            return -2.5 * ( 7.0 * polynomialParameter * polynomialParameter * polynomialParameter
                            - 3.0 * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 15.0 / 2.0 * ( - 1.0 + 7.0 * polynomialParameter * polynomialParameter )
                    * ( 1.0 - polynomialParameter * polynomialParameter );
        case 3:
            return -105.0 * polynomialParameter * ( 1.0 - polynomialParameter * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 4:
            return 105.0 * ( 1.0 - polynomialParameter * polynomialParameter )
                    * ( 1.0 - polynomialParameter * polynomialParameter );

        default:
            std::cerr << "Error, a explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    default:
        std::cerr << "Error, explicit legendre polynomial not possible for "
                  << degree << " " << order << std::endl;
    }
    return TUDAT_NAN;
}

//! Compute low degree/order geodesy-normalized Legendre polynomials explicitly.
double computeGeodesyLegendrePolynomialExplicit( const int degree,
                                                 const int order,
                                                 const double polynomialParameter )
{
    // If 0,0 term is requested return Legendre polynomial value.
    if ( degree == 0 && order == 0 )
    {
        return 1.0;
    }

    // Else if 1,0 term is requested return polynomial value.
    else if ( degree == 1 && order == 0 )
    {
        return std::sqrt( 3.0 ) * polynomialParameter;
    }

    // Else if 1,1 term is requested return polynomial value.
    else if ( degree == 1 && order == 1 )
    {
        return std::sqrt( 3.0 - 3.0 * polynomialParameter * polynomialParameter );
    }

    // Else the requested term cannot be computed; throw a run-time error.
    else
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage  <<  "Error: computation of Legendre polynomial of = "  <<  degree
                      <<  " and order = "  <<  order  <<  " is not supported."  <<  std::endl;

        // Throw a run-time error.
        boost::throw_exception( boost::enable_error_info( std::runtime_error(
                                                              errorMessage.str( ) ) ) );
    }
}

//! Compute unnormalized Legendre polynomial through sectoral recursion.
double computeLegendrePolynomialDiagonal( const int degree,
                                          const double degreeOneOrderOnePolynomial,
                                          const double priorSectoralPolynomial )
{
    // Return polynomial.
    return ( 2.0 * static_cast< double >( degree ) - 1.0 )
            * degreeOneOrderOnePolynomial * priorSectoralPolynomial;

}

//! Compute geodesy-normalized Legendre polynomial through sectoral recursion.
double computeGeodesyLegendrePolynomialDiagonal( const int degree,
                                                 const double degreeOneOrderOnePolynomial,
                                                 const double priorSectoralPolynomial )
{
    // Return polynomial.
    return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 )
                      / ( 6.0 * static_cast< double >( degree ) ) )
            * degreeOneOrderOnePolynomial * priorSectoralPolynomial;
}

//! Compute unnormalized Legendre polynomial through degree recursion.
double computeLegendrePolynomialVertical( const int degree,
                                          const int order,
                                          const double polynomialParameter,
                                          const double oneDegreePriorPolynomial,
                                          const double twoDegreesPriorPolynomial )
{
    // Return polynomial.
    return ( ( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter
             * oneDegreePriorPolynomial - ( static_cast< double >( degree + order ) - 1.0 )
             * twoDegreesPriorPolynomial ) / ( static_cast< double >( degree - order ) );
}

//! Compute geodesy-normalized Legendre polynomial through degree recursion.
double computeGeodesyLegendrePolynomialVertical( const int degree,
                                                 const int order,
                                                 const double polynomialParameter,
                                                 const double oneDegreePriorPolynomial,
                                                 const double twoDegreesPriorPolynomial )
{
    // Return polynomial.
    return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 )
                      / ( ( static_cast< double >( degree + order ) ) * ( static_cast< double >( degree - order ) ) ) )
            * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter * oneDegreePriorPolynomial
                - std::sqrt( ( static_cast< double >( degree + order ) - 1.0 )
                             * ( static_cast< double >( degree - order ) - 1.0 )
                             / ( 2.0 * static_cast< double >( degree ) - 3.0 ) )
                * twoDegreesPriorPolynomial );
}

//! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized.
double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order )
{

    double deltaFunction = 0.0;
    if( order == 0 )
    {
        deltaFunction = 1.0;
    }

    double factor = std::sqrt(
        boost::math::factorial< double >( static_cast< double >( degree + order ) )
        / ( ( 2.0 - deltaFunction ) * ( 2.0 * static_cast< double >( degree ) + 1.0 )
            * boost::math::factorial< double >( static_cast< double >( degree - order ) ) ) );
    return 1.0 / factor;
}

} // namespace basic_mathematics
} // namespace tudat


#include "tasktorrent/tasktorrent.hpp"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <memory>

#include <mpi.h>

typedef array<int, 2> int2;
typedef array<int, 3> int3;
typedef array<int, 4> int4;
typedef array<int, 5> int5;
typedef array<int, 6> int6;
typedef array<int, 7> int7;

struct scoped_timer {
  private:
    std::atomic<long long int>* time_us_;
    ttor::timer time_init_;
  public:
    scoped_timer(std::atomic<long long int>* time_us) {
        time_us_ = time_us;
        time_init_= ttor::wctime();
    }
    ~scoped_timer() {
        ttor::timer time_end_ = ttor::wctime();
        *time_us_ += static_cast<long long int>(1e6 * ttor::elapsed(time_init_, time_end_));
    }
};

ttor::view<double> make_view(Eigen::MatrixXd* A) {
    return ttor::view<double>(A->data(), A->size());
}

Eigen::MatrixXd make_from_view(ttor::view<double> A, int nrows) {
    Eigen::MatrixXd Add = Eigen::MatrixXd::Zero(nrows, nrows);
    assert(nrows * nrows == A.size());
    memcpy(Add.data(), A.data(), sizeof(double) * A.size());
    return Add;
}

void copy_from_view(Eigen::MatrixXd* dest, const ttor::view<double> A) {
    assert(dest->size() == A.size());
    memcpy(dest->data(), A.data(), sizeof(double) * A.size());
}

void accumulate(Eigen::MatrixXd* dest, const Eigen::MatrixXd* src) {
    assert(dest->size() == src->size());
    #pragma omp parallel for
    for(int k = 0; k < dest->size(); k++) {
        (*dest)(k) += (*src)(k);
    }
}

std::string to_string(int2 ij) {
    return to_string(ij[0]) + "_" + to_string(ij[1]);
}

std::string to_string(int3 ijk) {
    return to_string(ijk[0]) + "_" + to_string(ijk[1]) + "_" + to_string(ijk[2]);
}

/**
 * Matrix is of size N (global)
 * Each rank works on Nr x Nr
 * Each thread works on Nt x Nt
 *
 * There are n sub blocks on a given rank
 */
void gemm(const int N, const int Nt, const int n_threads, std::string logfile, const int verb, const bool test)
{
    const int rank = ttor::comm_rank();
    const int n_ranks = ttor::comm_size();
    const int n_ranks_1d = static_cast<int>(round(pow(n_ranks, 1.0/3.0)));
    assert(n_ranks_1d * n_ranks_1d * n_ranks_1d == n_ranks);
    const int rank_i = rank % n_ranks_1d;
    const int rank_j = (rank / n_ranks_1d) % n_ranks_1d;
    const int rank_k = rank / (n_ranks_1d * n_ranks_1d);
    const int3 rank_ijk = {rank_i, rank_j, rank_k};
    const int Nr = N / n_ranks_1d;
    assert(Nr * n_ranks_1d == N);
    const int n = Nr / Nt;
    assert(Nt * n == Nr);
    printf("Hello rank %d with 3d-index (%d %d %d) / (%d %d %d) from host %s, N %d, Nr %d, Nt %d, n %d\n", rank, rank_i, rank_j, rank_k, n_ranks_1d, n_ranks_1d, n_ranks_1d,
        ttor::processor_name().c_str(), N, Nr, Nt, n);

    printf("rank,%d\n", rank);
    printf("rank_i,%d\n", rank_i);
    printf("rank_j,%d\n", rank_j);
    printf("rank_k,%d\n", rank_k);
    printf("ntot,%d\n", N);
    printf("nrank,%d\n", Nr);
    printf("ntile,%d\n", Nt);
    printf("nthreads,%d\n", n_threads);
    printf("logfile,%s\n", logfile.c_str());
    printf("verb,%d\n", verb);
    printf("test,%d\n", test);

    auto rank_ijk_to_rank = [n_ranks_1d](int rank_i, int rank_j, int rank_k) {
        return rank_k * n_ranks_1d * n_ranks_1d + rank_j * n_ranks_1d + rank_i;
    };
    assert(rank_ijk_to_rank(rank_i, rank_j, rank_k) == rank);

    /**
     * Record timings
     **/
    std::atomic<long long int> send_copy_us_t(0);
    std::atomic<long long int> send_am_us_t(0);
    std::atomic<long long int> bcst_copy_us_t(0);
    std::atomic<long long int> bcst_am_us_t(0);
    std::atomic<long long int> gemm_us_t(0);
    std::atomic<long long int> gemm_copy_us_t(0);
    std::atomic<long long int> gemm_am_us_t(0);
    std::atomic<long long int> accu_us_t(0);

    /**
     * Original and final matrices
     **/
    // n x n matrix of Nt x Nt matrices, so Nr x Nr total
    std::vector<std::vector<Eigen::MatrixXd>> A_ij(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> C_ij(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> B_ij(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));

    auto val_global = [](int i, int j) { return static_cast<double>(1 + i + j); };
    if(rank_k == 0) {
        for(int i = 0; i < n; i++) {
            for(int j = 0; j < n; j++) {
                auto val = [&](int i_, int j_) { return val_global(rank_i * Nr + i * Nt + i_, rank_j * Nr + j * Nt + j_); };
                A_ij[i][j] = Eigen::MatrixXd::NullaryExpr(Nt, Nt, val);
                B_ij[i][j] = Eigen::MatrixXd::NullaryExpr(Nt, Nt, val);
            }
        }
    }

    /**
     * Workspace
     **/
    std::vector<std::vector<Eigen::MatrixXd>> A_ijk(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> C_ijk(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> B_ijk(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::unique_ptr<std::atomic<int>>> C_ijk_counts(n * n);
    for(int i = 0; i < n*n; i++) {
        C_ijk_counts[i] = std::make_unique<std::atomic<int>>();
        C_ijk_counts[i]->store(0);
    }

    // C_ijk_accu[sub_i][sub_j][from] stores the results for (sub_i, sub_j) to be accumulated, from rank from
    std::vector<std::vector<std::vector<Eigen::MatrixXd>>> C_ijk_accu(
        n, std::vector<std::vector<Eigen::MatrixXd>>(
        n, std::vector<Eigen::MatrixXd>(
        n_ranks_1d, Eigen::MatrixXd::Zero(Nt, Nt))));

    MPI_Barrier(MPI_COMM_WORLD);

    /**
     * Initialize the runtime structures
     **/
    ttor::Communicator comm(MPI_COMM_WORLD, verb);
    ttor::Threadpool tp(n_threads, &comm, verb, "Wk_Gemm_" + to_string(rank) + "_");

    // send is indexed by int2, which are the sub blocks
    ttor::Taskflow<int2> send_Aij(&tp, verb);  // (i,j,0) sends A_ij to (i,j,j) for all i,j
    ttor::Taskflow<int2> send_Bij(&tp, verb);  // (i,j,0) sends B_ij to (i,j,i) for all i,j
    // send is indexed by int2, which are the sub blocks
    ttor::Taskflow<int2> bcst_Aij(&tp, verb);  // (i,j,j) sends A_ij along j to all (i,*,j) for all i,j
    ttor::Taskflow<int2> bcst_Bij(&tp, verb);  // (i,j,i) sends B_ij along i to all (*,j,i) for all i,j
    // gemm is indexed by int3, which are the sub blocks
    ttor::Taskflow<int3> gemm_Cijk(&tp, verb); // (i,j,k) compute C_ijk = A_ik * B_kj, send for accumulation reduction on (i,j,0)
    ttor::Taskflow<int3> accu_Cij(&tp, verb);  // accumulate (i,j,from) into (i,j)

    ttor::Logger log(1000000);
    if(logfile.size() > 0) {
        tp.set_logger(&log);
        comm.set_logger(&log);
    }

    /**
     * Send
     **/

    auto send_Aij_am = comm.make_active_msg([&](ttor::view<double>& Aij, int& sub_i, int& sub_j) {
        scoped_timer t(&send_am_us_t);
        copy_from_view(&A_ijk[sub_i][sub_j], Aij);
        bcst_Aij.fulfill_promise({sub_i, sub_j});
    });

    auto send_Bij_am = comm.make_active_msg([&](ttor::view<double>& Bij, int& sub_i, int& sub_j) {
        scoped_timer t(&send_am_us_t);
        copy_from_view(&B_ijk[sub_i][sub_j], Bij);
        bcst_Bij.fulfill_promise({sub_i, sub_j});
    });

    // (i,j,0) sends A_ij to (i,j,j) for all i,j
    send_Aij.set_task([&](int2 sub_ij){
        assert(rank_k == 0);
        scoped_timer t(&send_copy_us_t);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> A_view = make_view(&A_ij[sub_i][sub_j]);
        int dest = rank_ijk_to_rank(rank_i, rank_j, rank_j);
        if(dest == rank) {
            A_ijk[sub_i][sub_j] = A_ij[sub_i][sub_j];
            bcst_Aij.fulfill_promise({sub_i, sub_j});
        } else {
            send_Aij_am->send(dest, A_view, sub_i, sub_j);
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return sub_ij[0] % n_threads;
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[1]);
    }).set_binding([&](int2 sub_ij){
        return false;
        // return true;
    }).set_name([&](int2 sub_ij) { return "send_A_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    // (i,j,0) sends B_ij to (i,j,i) for all i,j
    send_Bij.set_task([&](int2 sub_ij){
        assert(rank_k == 0);
        scoped_timer t(&send_copy_us_t);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> B_view = make_view(&B_ij[sub_i][sub_j]);
        int dest = rank_ijk_to_rank(rank_i, rank_j, rank_i);
        if(dest == rank) {
            B_ijk[sub_i][sub_j] = B_ij[sub_i][sub_j];
            bcst_Bij.fulfill_promise({sub_i, sub_j});
        } else {
            send_Bij_am->send(dest, B_view, sub_i, sub_j);
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return (sub_ij[1] % n_threads);
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[0]);
    }).set_binding([&](int2 sub_ij){
        return false;
        //return true;
    }).set_name([&](int2 sub_ij) { return "send_B_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    /**
     * Broadcast
     **/

    auto bcst_Aij_am = comm.make_active_msg([&](ttor::view<double>& Aij, int &sub_i, int &sub_j) {
        scoped_timer t(&bcst_am_us_t);
        copy_from_view(&A_ijk[sub_i][sub_j], Aij);
        for(int k = 0; k < n; k++)
            gemm_Cijk.fulfill_promise({sub_i, k, sub_j});
    });

    auto bcst_Bij_am = comm.make_active_msg([&](ttor::view<double>& Bij, int &sub_i, int &sub_j) {
        scoped_timer t(&bcst_am_us_t);
        copy_from_view(&B_ijk[sub_i][sub_j], Bij);
        for(int k = 0; k < n; k++)
            gemm_Cijk.fulfill_promise({k, sub_j, sub_i});
    });

    // (i,j,j) sends A_ij along j to all (i,*,j) for all i,j
    bcst_Aij.set_task([&](int2 sub_ij){
        scoped_timer t(&bcst_copy_us_t);
        assert(rank_j == rank_k);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> A_view = make_view(&A_ijk[sub_i][sub_j]);
        for(int k = 0; k < n_ranks_1d; k++) {
            int dest = rank_ijk_to_rank(rank_i, k, rank_j);
            if(dest == rank) {
                for(int l = 0; l < n; l++)
                    gemm_Cijk.fulfill_promise({sub_i, l, sub_j});
            } else {
                bcst_Aij_am->send(dest, A_view, sub_i, sub_j);
            }
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return (sub_ij[0] % n_threads);
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[1]);
    }).set_binding([&](int2 sub_ij){
        return false;
        //return true;
    }).set_name([&](int2 sub_ij) { return "bcast_A_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    // (i,j,i) sends B_ij along i to all (*,j,i) for all i,j
    bcst_Bij.set_task([&](int2 sub_ij){
        scoped_timer t(&bcst_copy_us_t);
        assert(rank_i == rank_k);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> B_view = make_view(&B_ijk[sub_i][sub_j]);
        for(int k = 0; k < n_ranks_1d; k++) {
            int dest = rank_ijk_to_rank(k, rank_j, rank_i);
            if(dest == rank) {
                for(int l = 0; l < n; l++)
                    gemm_Cijk.fulfill_promise({l, sub_j, sub_i});
            } else {
                bcst_Bij_am->send(dest, B_view, sub_i, sub_j);
            }
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return (sub_ij[1] % n_threads);
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[0]);
    }).set_binding([&](int2 sub_ij){
        return false;
        // return true;
    }).set_name([&](int2 sub_ij) { return "bcast_B_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    /**
     * GEMM
     **/

    auto gemm_Cijk_am = comm.make_active_msg([&](ttor::view<double>& Cijk, int &sub_i, int &sub_j, int& from) {
        scoped_timer t(&gemm_am_us_t);
        copy_from_view(&C_ijk_accu[sub_i][sub_j][from], Cijk);
        accu_Cij.fulfill_promise({sub_i, sub_j, from});
    });

    // (i,j,k) compute C_ijk = A_ik * B_kj
    gemm_Cijk.set_task([&](int3 sub_ijk){
        int sub_i = sub_ijk[0];
        int sub_j = sub_ijk[1];
        int sub_k = sub_ijk[2];
        {
            scoped_timer t(&gemm_us_t);
            C_ijk[sub_i][sub_j].noalias() += A_ijk[sub_i][sub_k] * B_ijk[sub_k][sub_j];
        }
        //(*(C_ijk_counts[sub_i * n + sub_j]))++;
        scoped_timer t(&gemm_copy_us_t);
        if(sub_ijk[2] < n-1) {
            gemm_Cijk.fulfill_promise({sub_i, sub_j, sub_k+1});
        } else {
        //if(C_ijk_counts[sub_i * n + sub_j]->load() == n) {
            auto C_ijk_view = make_view(&C_ijk[sub_i][sub_j]);
            int dest = rank_ijk_to_rank(rank_i, rank_j, 0);
            if(dest == rank) {
                C_ijk_accu[sub_i][sub_j][rank_k] = C_ijk[sub_i][sub_j];
                accu_Cij.fulfill_promise({sub_i, sub_j, rank_k});
            } else {
                int k = rank_k;
                gemm_Cijk_am->send(dest, C_ijk_view, sub_i, sub_j, k);
            }
        }
    }).set_indegree([&](int3 sub_ijk) {
        return sub_ijk[2] == 0 ? 2 : 3; // 2 A_ik and B_kj blocks, + previous gemm
        // return 2;
    }).set_mapping([&](int3 sub_ijk) {
        if(n_threads == 1) return 0;
        // else return max(1, (sub_ijk[0] + sub_ijk[1] * n) % n_threads);
        else return max(1, (sub_ijk[0] + sub_ijk[1] * n) % n_threads); // + sub_ijk[2] * n * n) % n_threads);
        // return (sub_ijk[0] + sub_ijk[1] * n + sub_ijk[2] * n * n) % n_threads;
    }).set_binding([&](int3 sub_ijk) {
        return false;
    }).set_priority([&](int3 sub_ijk){
        return 0.0 * n + (n - sub_ijk[2]);
    }).set_name([&](int3 sub_ijk) { return "gemm_C_" + to_string(sub_ijk) + "_" + to_string(rank_ijk); });

    // (i,j,k) compute C_ijk = A_ik * B_kj
    accu_Cij.set_task([&](int3 sub_ij_from){
        scoped_timer t(&accu_us_t);
        int sub_i = sub_ij_from[0];
        int sub_j = sub_ij_from[1];
        int from  = sub_ij_from[2];
        C_ij[sub_i][sub_j] += C_ijk_accu[sub_i][sub_j][from];
    }).set_indegree([&](int3) {
        return 1;
    }).set_mapping([&](int3 sub_ij_from) {
        if(n_threads == 1) return 0;
        else return max(1, (sub_ij_from[0] + n * sub_ij_from[1]) % n_threads);
        // return (sub_ij_from[0] + n * sub_ij_from[1]) % n_threads;
    }).set_binding([&](int3) {
        return true;
    }).set_priority([&](int3){
        return 0.0;
    }).set_name([&](int3 sub_ij_from) { return "accu_C_" + to_string(sub_ij_from) + "_" + to_string(rank_ijk); });

    printf("Starting 3D Gemm...\n");
    ttor::timer t0 = ttor::wctime();
    if(rank_k == 0) {
        for(int i = 0; i < n; i++) {
            for(int j = 0; j < n; j++) {
                send_Aij.fulfill_promise({i,j});
                send_Bij.fulfill_promise({i,j});
            }
        }
    }
    tp.join();
    ttor::timer t1 = ttor::wctime();
    double total_time = ttor::elapsed(t0, t1);
    double gemm_time = gemm_us_t.load() * 1e-6;
    double gemm_time_per_thread = gemm_time / n_threads;
    printf("Done\n");
    printf("total_time,%e\n", ttor::elapsed(t0, t1));
    printf("send_copy_us_t,%e\n",send_copy_us_t.load() * 1e-6);
    printf("send_am_us_t,%e\n",send_am_us_t.load() * 1e-6);
    printf("bcst_copy_us_t,%e\n",bcst_copy_us_t.load() * 1e-6);
    printf("bcst_am_us_t,%e\n",bcst_am_us_t.load() * 1e-6);
    printf("gemm_us_t,%e\n",gemm_us_t.load() * 1e-6);
    printf("gemm_copy_us_t,%e\n",gemm_copy_us_t.load() * 1e-6);
    printf("gemm_am_us_t,%e\n",gemm_am_us_t.load() * 1e-6);
    printf("accu_us_t,%e\n",accu_us_t.load() * 1e-6);
    // For easy CSV parsing
    printf("[rank]>>>>Ntot,Nrank,Ntile,rank,n_ranks,nthreads,tot_time,gemm_time,gemm_time_per_thread\n");
    printf("[%d]>>>>%d,%d,%d,%d,%d,%d,%e,%e,%e\n",rank,N,Nr,Nt,rank,n_ranks,n_threads,total_time,gemm_time,gemm_time_per_thread);

    if(logfile.size() > 0) {
        std::ofstream logstream;
        std::string filename = logfile + ".log." + to_string(rank);
        printf("Saving log to %s\n", filename.c_str());
        logstream.open(filename);
        logstream << log;
        logstream.close();
    }

    MPI_Barrier(MPI_COMM_WORLD);
    if(test && rank_k == 0) {
        // Send all to 0
        int n_received = 0;
        int n_expected = (rank == 0 ? n * n * n_ranks_1d * n_ranks_1d : 0);
        Eigen::MatrixXd C_test = Eigen::MatrixXd::Zero(N, N);
        ttor::Communicator comm(MPI_COMM_WORLD, verb);
        auto am = comm.make_active_msg([&](ttor::view<double>& A, int& rank_i_from, int& rank_j_from, int& sub_i, int& sub_j){
            C_test.block(rank_i_from * Nr + sub_i * Nt, rank_j_from * Nr + sub_j * Nt, Nt, Nt) = make_from_view(A, Nt);
            n_received++;
        });
        int rank_i_from = rank_i;
        int rank_j_from = rank_j;
        for(int sub_i = 0; sub_i < n; sub_i++) {
            for(int sub_j = 0; sub_j < n; sub_j++) {
                auto C_view = make_view(&C_ij[sub_i][sub_j]);
                am->send(0, C_view, rank_i_from, rank_j_from, sub_i, sub_j);
            }
        }
        while((!comm.is_done()) || (n_received < n_expected)) {
            comm.progress();
        }
        // Compute reference on 0
        if(rank == 0) {
            Eigen::MatrixXd A_ref = Eigen::MatrixXd::NullaryExpr(N, N, val_global);
            Eigen::MatrixXd B_ref = Eigen::MatrixXd::NullaryExpr(N, N, val_global);
            ttor::timer t0 = ttor::wctime();
            Eigen::MatrixXd C_ref = A_ref * B_ref;
            ttor::timer t1 = ttor::wctime();
            double error = (C_ref - C_test).norm() / C_ref.norm();
            printf("\n==> GEMM error %e\n\n", error);
            printf("Reference code took %e\n", ttor::elapsed(t0, t1));
            assert(error <= 1e-12);
        }
    }
}

int main(int argc, char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    int N = 128;
    int Nt = 8;
    int n_threads = 1;
    int verb = 0;
    bool test = true;
    std::string logfile = "";

    if (argc >= 2)
    {
        N = atoi(argv[1]);
        assert(N > 0);
    }

    if (argc >= 3) {
        Nt = atoi(argv[2]);
        assert(Nt > 0);
        assert(Nt <= N);
    }

    if (argc >= 4) {
        n_threads = atoi(argv[3]);
        assert(n_threads > 0);
    }

    if (argc >= 5) {
        logfile = argv[4];
    }

    if (argc >= 6) {
        verb = atoi(argv[5]);
        assert(verb >= 0);
    }

    if (argc >= 7) {
        test = static_cast<bool>(atoi(argv[6]));
    }

    if(ttor::comm_rank() == 0) printf("Usage: ./3d_gemm N Nt n_threads logfile verb test\n");
    if(ttor::comm_rank() == 0) printf("Arguments: N (global matrix size) %d, Nt (smallest block size) %d, n_threads %d, logfile %s, verb %d, test %d\n", N, Nt, n_threads, logfile.c_str(), verb, test);

    gemm(N, Nt, n_threads, logfile, verb, test);

    MPI_Finalize();
}


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::find_scale_to_finite_sum.hpp                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_FIND_SCALE_TO_FINITE_SUM_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_FIND_SCALE_TO_FINITE_SUM_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <stdexcept>
#include <boost/lambda/lambda.hpp>
#include <boost/range.hpp>
#include <boost/format.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/statistics/detail/importance_sampling/weights/maximal_finite_sums.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace importance_sampling{

    // Finds c such that the sum over sum{*i/c, i in [b,e)} < inf using the
    // bisection method.
    //
    // Warning: The value c is not insensitive to permutations of [b,e), due
    // to non-associativity in the fp system
    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(
        InIt b,InIt e,
        typename iterator_value<InIt>::type low_init,
        typename iterator_value<InIt>::type high_init
    );

    // This version may be faster than that above with low = 1, high = highest
    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(InIt b,InIt e);

    // Implementation //

    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(
        InIt b,InIt e,
        typename iterator_value<InIt>::type low_init,
        typename iterator_value<InIt>::type high_init
    ){
        typedef typename iterator_value<InIt>::type val_;
        static val_ zero = static_cast<val_>(0);
        static val_ two = static_cast<val_>(2);
        static val_ eps = boost::math::tools::epsilon<val_>();

        BOOST_ASSERT(low_init < high_init);
        BOOST_ASSERT(low_init > zero);
        BOOST_ASSERT(high_init > zero);
        val_ low = low_init;
        val_ high = high_init;
        val_ mid = (low + high)/two;
        val_ delta, acc;

        do{
            delta = high - low;
            acc =
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / mid) );
            if(boost::math::isinf(acc)){
                low = mid;
            }else{
                high = mid;
            }
            mid = (low+high)/two;
        }while(
            delta - (high - low)>eps
        );
        static
            const char* str = "%3% = find_scale_to_finite_sum(b,e,%1%,%2%) = inf";
        if(
            boost::math::isinf(
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / high) )
            )
        ){
            format f(str); f%low_init%high_init%high;
            throw std::runtime_error(f.str());
        }
        return high;
    }

    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(InIt b,InIt e){
        typedef typename iterator_value<InIt>::type val_;
        typedef numeric::bounds<val_>               bounds_;
        typedef std::vector<val_>                   vec_;
        static val_ zero = static_cast<val_>(0);
        static val_ one = static_cast<val_>(1);
        static val_ two = static_cast<val_>(2);
        static val_ highest = bounds_::highest();
        vec_ vec;
        maximal_finite_sums(b,e,std::back_inserter(vec));
        val_ low = one;
        val_ high = highest;
        val_ mid = find_scale_to_finite_sum(
            boost::begin(vec),
            boost::end(vec),
            low,high
        );
        low = mid;
        high = mid;
        while(
            !boost::math::isinf(
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / low) )
            )
        ){
            low /= two;
            if(low<one){
                low = one;
                break;
            }
        }
        while(
            boost::math::isinf(
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / high) )
            )
        ){
            high *= two;
            if(boost::math::isinf(high)){
                high = highest;
                break;
            }
        }
        return find_scale_to_finite_sum(b,e,low,high);
    }

}// importance_weights
}// detail
}// statistics
}// boost

#endif


#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <memory>
#include <sstream>
#include <unordered_map>
#include <unordered_set>
#include <vector>

using Registers = std::unordered_map<std::string, int>;

Registers reg;
int mm = 0;

int& getReg(const std::string& name) {
    return reg.insert({name, 0}).first->second;
}

bool eval_cond(const std::vector<std::string>& words)
{
    if (words[3] == "if") {
        int& r = getReg(words[4]);
        int val = stoi(words[6]);
        std::cout << ", CR: " << words[4] << ", op: " << words[5] << ", val: " << val;
        if (words[5] == "==") {
            return r == val;
        } else if (words[5] == ">=") {
            return r >= val;
        } else if (words[5] == "<=") {
            return r <= val;
        } else if (words[5] == "!=") {
            return r != val;
        } else if (words[5] == ">") {
            return r > val;
        } else if (words[5] == "<") {
            return r < val;
        } else {
            std::cerr << "UNKNOWN CONDITIONAL OP: " << words[5] << "\n";
            exit(1);
        }
    } else {
        std::cerr << "UNKNOWN CONDITIONAL: " << words[3] << "\n";
        exit(1);
    }
}

int main()
{
    std::string line;
    while (std::getline(std::cin, line)) {
        if (line.empty()) {
            break;
        }
        std::vector<std::string> words;
        boost::algorithm::split(
            words, line, boost::is_any_of(" "), boost::algorithm::token_compress_on);

        int& r = getReg(words[0]);
        int val = stoi(words[2]);

        std::cout << "R: " << words[0] << ", op: " << words[1] << ", val: " << val;
        if (words.size() > 3 && !eval_cond(words)) {
            std::cout << " skip\n";
            continue;
        }

        if (words[1] == "inc") {
            std::cout << ", INC\n";
            r += val;
        } else if (words[1] == "dec") {
            std::cout << ", DEC\n";
            r -= val;
        } else {
            std::cerr << "UNKNOWN OP: " << words[1] << "\n";
            exit(1);
        }
        mm = std::max(mm, r);
    }

    auto m = std::max_element(
        reg.begin(), reg.end(), [](const Registers::value_type& a, const Registers::value_type& b) {
            return std::less<int>()(a.second, b.second);
        });

    std::cout << "max: " << m->second << ", mm: " << mm << "\n";
    return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_BETA_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BETA_RNG_HPP

#include <boost/math/special_functions/gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/log_sum_exp.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>

namespace stan {
  namespace math {

    /**
     * Return a pseudorandom Beta variate with the supplied success and failure
     * parameters and specified random number generator.
     *
     * @tparam RNG class of random number generator
     * @param alpha positive finite success parameter
     * @param beta positive finite failure parameter
     * @param rng random number generator
     * @return Beta random variate
     * @throw std::domain_error if alpha or beta is nonpositive
     */
    template <class RNG>
    inline double
    beta_rng(double alpha,
             double beta,
             RNG& rng) {
      using boost::variate_generator;
      using boost::random::gamma_distribution;
      using boost::random::uniform_real_distribution;
      using std::log;
      using std::exp;
      static const char* function("beta_rng");
      check_positive_finite(function, "First shape parameter", alpha);
      check_positive_finite(function, "Second shape parameter", beta);

      // If alpha and beta are large, trust the usual ratio of gammas
      // method for generating beta random variables. If any parameter
      // is small, work in log space and use Marsaglia and Tsang's trick
      if (alpha > 1.0 && beta > 1.0) {
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_alpha(rng, gamma_distribution<>(alpha, 1.0));
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_beta(rng, gamma_distribution<>(beta, 1.0));
        double a = rng_gamma_alpha();
        double b = rng_gamma_beta();
        return a / (a + b);
      } else {
        variate_generator<RNG&, uniform_real_distribution<> >
          uniform_rng(rng, uniform_real_distribution<>(0.0, 1.0));
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_alpha(rng, gamma_distribution<>(alpha + 1, 1.0));
        variate_generator<RNG&, gamma_distribution<> >
          rng_gamma_beta(rng, gamma_distribution<>(beta + 1, 1.0));
        double log_a = log(uniform_rng()) / alpha + log(rng_gamma_alpha());
        double log_b = log(uniform_rng()) / beta + log(rng_gamma_beta());
        double log_sum = log_sum_exp(log_a, log_b);
        return exp(log_a - log_sum);
      }
    }

  }
}
#endif


#include "RenderMesh.h"

#include <boost/test/unit_test.hpp>
#include <Eigen/Geometry>

using namespace Eigen;
using namespace std;

BOOST_AUTO_TEST_CASE(RenderMesh_Transform_Identity)
{
	BOOST_CHECK_EQUAL
		( ::Transform(Matrix4f::Identity(), Vector3f(1.0f, 2.0f, 3.0f))
		, Vector3f(1.0f, 2.0f, 3.0f)
		);
}

BOOST_AUTO_TEST_CASE(RenderMesh_Tranform_Scale)
{
	BOOST_CHECK_EQUAL
		( ::Transform(Affine3f(Translation3f(4.0f, 5.0f, 6.0f)).matrix(), Vector3f(1.0f, 2.0f, 3.0f))
		, Vector3f(5.0f, 7.0f, 9.0f)
		);
}


#include <sstream>
#include <string>
#include <set>
#include <exception>
#include <iostream>

// Boost property tree
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/foreach.hpp>

// Boost program options
#include "boost/program_options.hpp"

// Boost.Geometry
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/register/segment.hpp>

using namespace boost::geometry;
namespace pt = boost::property_tree;

typedef model::d2::point_xy<double> Point;
typedef model::polygon<Point, false, false> Polygon;
typedef model::segment<Point> Segment;

Polygon operator+(
  const Polygon& poly,
  const Point& offset)
{
  Polygon result;
  result.outer().reserve(poly.outer().size());
  for (auto&& p : poly.outer()) {
    append(result, Point(p.x() + offset.x(), p.y() + offset.y()));
  }
  return result;
}

double run(
  const std::string& sceneFile,
  const std::string& outputFile)
{
  // Declare a stream and an SVG mapper
  std::ofstream svg(outputFile);
  boost::geometry::svg_mapper<Point> mapper(svg, 400, 400, "width=\"400\" height=\"400\"");

  // Create empty property tree object
  pt::ptree tree;

  // Parse the JSON into the property tree.
  pt::read_json(sceneFile, tree);

  // read boundary
  Polygon poly;
  boost::geometry::read_wkt(tree.get<std::string>("boundary"), poly);
  mapper.add(poly);
  mapper.map(poly, "fill-opacity:1.0;fill:none;stroke:rgb(0,0,0);stroke-width:5");

  // read targets
  std::vector<Polygon> objectPaths;
  std::vector<double> objectVelocities;

  for (auto& item : tree.get_child("targets")) {
    Polygon shape;
    boost::geometry::read_wkt(item.second.get<std::string>("shape"), shape);
    Polygon path;
    boost::geometry::read_wkt(item.second.get<std::string>("path"), path);
    double vel = item.second.get<double>("velocity", 1.0);

    mapper.map(shape + path.outer().front(), "fill-opacity:1.0;fill:rgb(153,153,153)");
    mapper.map(path, "fill-opacity:0.8;fill:none;stroke:rgb(0,0,255);stroke-width:2");

    if (path.outer().size() > 1) {
      std::stringstream sstr;
      sstr << vel << " m/s";
      mapper.text(path.outer().front(), sstr.str(), "fill:rgb(0,0,0);font-family:Arial;font-size:15px");
    }
  }

  // read obstacles
  for (auto& item : tree.get_child("obstacles")) {
    Polygon shape;
    boost::geometry::read_wkt(item.second.get<std::string>("shape"), shape);
    Polygon path;
    boost::geometry::read_wkt(item.second.get<std::string>("path"), path);
    double vel = item.second.get<double>("velocity", 1.0);

    mapper.map(shape + path.outer().front(), "fill-opacity:1.0;fill:rgb(80,80,80)");
    mapper.map(path, "fill-opacity:0.8;fill:none;stroke:rgb(0,0,255);stroke-width:2");

    if (path.outer().size() > 1) {
      std::stringstream sstr;
      sstr << vel << " m/s";
      mapper.text(path.outer().front(), sstr.str(), "fill:rgb(0,0,0);font-family:Arial;font-size:15px");
    }
  }

  // read cameras
  double cameraFoV = tree.get<double>("cameraFoV") / 180 * M_PI;
  for (auto& item : tree.get_child("cameras")) {
    Point pos;
    boost::geometry::read_wkt(item.second.get<std::string>("pos"), pos);
    double angle = item.second.get<double>("angle") / 180 * M_PI;
    double maxVelocity = item.second.get<double>("maxVelocity");
    double maxAngularVelocity = item.second.get<double>("maxAngularVelocity");

    boost::geometry::model::linestring<Point> polyline;
    const double length = 2;
    append(polyline, Point(length * cos(angle - cameraFoV) + pos.x(), length * sin(angle - cameraFoV) + pos.y()));
    append(polyline, pos);
    append(polyline, Point(length * cos(angle + cameraFoV) + pos.x(), length * sin(angle + cameraFoV) + pos.y()));
    mapper.map(pos, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:2", 5);
    mapper.map(polyline, "fill-opacity:0.8;fill:none;stroke:rgb(153,204,0);stroke-width:2");

    std::stringstream sstr;
    sstr << maxVelocity << " m/s\n" << maxAngularVelocity << " rad/s";
    mapper.text(pos, sstr.str(), "fill:rgb(0,0,0);font-family:Arial;font-size:15px");
  }
}

int main(int argc, char** argv)
{
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
      ("help", "produce help message")
      ("scene,s", po::value<std::string>()->required(), "input file for scene")
      ("output,o", po::value<std::string>()->required(), "output file")
  ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);

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

  po::notify(vm);

  run(
    vm["scene"].as<std::string>(),
    vm["output"].as<std::string>());

  return 0;
}


/*  Sirikata
 *  ExpIntegral.cpp
 *
 *  Copyright (c) 2010, Daniel Reiter Horn
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are
 *  met:
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *  * Neither the name of Sirikata nor the names of its contributors may
 *    be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <sirikata/core/util/Platform.hpp>
#include "ExpIntegral.hpp"

#ifndef SIRIKATA_BAD_BOOST_ERF
#include <boost/math/special_functions/erf.hpp>
#endif

#include <iostream>
#include <string.h>

static double square(double x) {
    return x*x;
}

static double expFunction(double k,double x, double y, double u, double v) {
    return exp(-k*(square(x-u)+square(y-v)));
}
static double myerf(double value) {
#ifndef SIRIKATA_BAD_BOOST_ERF
    return boost::math::erf(value);
#else
    return 0;
#endif
}
static double integralExpFunction(double k, double xmin, double xmax, double ymin, double ymax, double umin, double umax, double vmin, double vmax) {
    double e=2.7182818284590451;
    double j=xmin;
    double l=xmax;
    double m=ymin;
    double n=ymax;
    double p=umin;
    double q=umax;
    double r=vmin;
    double s=vmax;
    double pi=3.1415926535897931;
    double sqrtloge=1;
    double loge=1;
    return (1./(4*k*log(e)))
        *pi*(  ( j - p)*myerf(sqrt(k)*(j - p)*sqrtloge)
             + (-l + p)*myerf(sqrt(k)*(l - p)*sqrtloge)
             + (-j + q)*myerf(sqrt(k)*(j - q)*sqrtloge)
             + ( l - q)*myerf(sqrt(k)*(l - q)*sqrtloge)
               + (exp(-k*square(j - p))
                  - exp(-k*square(j - q)))/(sqrt(k)*sqrt(pi)*sqrtloge) +
               (-exp(-k*square(l - p)) + exp(-k*square(l - q)))/(sqrt(k)*sqrt(pi)*sqrtloge))
        *(  ( m - r)*myerf(sqrt(k)*(m - r)*sqrtloge)
            + (-n + r)*myerf(sqrt(k)*(n - r)*sqrtloge)
            + (-m + s)*myerf(sqrt(k)*(m - s)*sqrtloge)
            + ( n - s)*myerf(sqrt(k)*(n - s)*sqrtloge)
            + (exp(-k*square(m - r)) - exp(-k*square(m - s)))/(sqrt(k)*sqrt(pi)*sqrtloge)
            + (-exp(-k*square(n - r)) + exp(-k*square(n - s)))/(sqrt(k)*sqrt(pi)*sqrtloge));
}
namespace Sirikata {
double integralExpFunction(double k, const Vector3d& xymin, const Vector3d& xymax, const Vector3d& uvmin, const Vector3d& uvmax){
    return ::integralExpFunction(k,xymin.x,xymax.x,xymin.y,xymax.y,uvmin.x,uvmax.x,uvmin.y,uvmax.y);
}
}
int maino (int argc, char**argv) {
    float k=atof(argv[1]);

    float xmin=-1;
    float xmax= 1;
    float ymin=-1;
    float ymax= 1;
    float arg=atof(argv[2]);
    float umin=-arg;
    float umax= arg;
    float vmin=-arg;
    float vmax= arg;

    std::cout << " Function evaluated at hundred: "<<
        expFunction(1,100,100,100,100)<<
        " Integration of function over a lot "<<integralExpFunction(k,
                                xmin,
                                xmax,
                                ymin,
                                ymax,
                                umin,
                                umax,
                                vmin,
                                vmax);
    return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/detail/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/detail/constant/pio_4lo.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/ifnot_plus.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

  template < class A0 >
  struct invtrig_base<A0,tag::radian_tag,tag::simd_type, float>
  {
    static BOOST_FORCEINLINE A0 asin(A0 const& a0)
    {
      A0 sgn, x;
      x = bs::abs(a0);
      sgn = bs::bitofsign(a0);
      const auto x_larger_05 = x > bs::Half<A0>();
      A0 z = if_else(x_larger_05, bs::Half<A0>()*bs::oneminus(x), bs::sqr(x));
      x = if_else(x_larger_05, sqrt(z), x);
      // Remez polynomial of degree 4 for (asin(rx)-rx)/(rx*rx*rx) in [0, 0.25]
      // 2120752146 values (99.53%) within 0.0 ULPs
      //    9954286 values (0.47%)  within 0.5 ULPs
      // 4.0 cycles/element SSE4.2 g++-4.8
      A0 z1 = horn<A0,
        0x3e2aaae4,
        0x3d9980f6,
        0x3d3a3ec7,
        0x3cc617e3,
        0x3d2cb352
        > (z);
      z1 = bs::fma(z1, z*x, x);
      z = if_else(x_larger_05, bs::Pio_2<A0>()-(z1+z1), z1);
      return bs::bitwise_xor(z, sgn);
    }

    static BOOST_FORCEINLINE A0 acos(const A0& a0)
    {
      // 2130706432 values computed.
      // 1968272987 values (92.38%) within 0.0 ULPs
      //  162433445 values (7.62%)  within 0.5 ULPs
      // 8.5 cycles/element SSE4.2 g++-4.8
      A0 x = bs::abs(a0);
      auto x_larger_05 = is_greater(x, bs::Half<A0>());
      x  = if_else(x_larger_05, bs::sqrt(fma(bs::Mhalf<A0>(), x, bs::Half<A0>())), a0);
      x  = asin(x);
      x =  if_plus(x_larger_05, x, x);
      x  = bs::if_else(a0 < bs::Mhalf<A0>(), bs::Pi<A0>()-x, x);
      return bs::if_else(x_larger_05, x, bs::Pio_2<A0>()-x);
    }

    static BOOST_FORCEINLINE A0 atan(const A0& a0)
    {
      A0 absa0 =  bs::abs(a0);
      const A0 x  = kernel_atan(absa0, bs::rec(absa0));
      return bs::bitwise_xor(x, bs::bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 acot(const A0& a0)
    {
      A0 absa0 =  bs::abs(a0);
      const A0 x  = kernel_atan(bs::rec(absa0), absa0);
      return bs::bitwise_xor(x, bs::bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 kernel_atan(const A0&  x, const A0& recx)
    {
      //4278190076 values computed  in range: [-3.40282e+38, 3.40282e+38]
      //4257598358 values (99.52%)  within 0.0 ULPs
      //  20591718 values (0.48%)   within 0.5 ULPs

      //here x is positive
      const auto flag1 = x < Tan_3pio_8<A0>();
      const auto flag2 = bs::logical_and(x >= Constant<A0, 0x3ed413cd>(), flag1);
      A0 yy =  bs::if_zero_else(flag1, Pio_2<A0>());
      yy =  bs::if_else(flag2, Pio_4<A0>(), yy);
      A0 xx =   bs::if_else(flag1, x, -recx);
      xx =  bs::if_else(flag2, (bs::dec(x)/bs::inc(x)),xx);
      const A0 z = bs::sqr(xx);
      A0 z1 = horn<A0
        , 0xbeaaaa2aul  // -3.3333293e-01
        , 0x3e4c925ful  //  1.9991724e-01
        , 0xbe0e1b85ul  // -1.4031009e-01
        , 0x3da4f0d1ul  //  8.5460119e-02
        > (z);
      z1 = bs::fma(xx, bs::multiplies( z1, z), xx);
      z1 = if_plus(flag2, z1, Pio_4lo<A0>());
      z1 = ifnot_plus(flag1, z1, Pio_2lo<A0>());
      return yy+z1;
    }
  };
}
} }
#endif


#include "vtkReferenceFrameFilter.h"
#include <vtkImageData.h>
#include <vtkObjectFactory.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkInformationVector.h>
#include <vtkInformation.h>
#include <vtkDataObject.h>
#include <vtkSmartPointer.h>
#include <vtkPointData.h>
#include <vtkDataArray.h>
#include <vtkVector.h>
#include <Eigen\Core>
#include <Eigen\StdVector>
#include <Eigen\QR>
#include <vector>
#include <vtkFloatArray.h>

vtkStandardNewMacro(vtkReferenceFrameFilter);

// Creates a 2x2 matrix from 2 column vectors
static Eigen::Matrix2d make_Matrix2d(const Eigen::Vector2d& c0, const Eigen::Vector2d& c1)
{
	Eigen::Matrix2d M;
	M << c0.x(), c1.x(), c0.y(), c1.y();
	return M;
}

// Create a 2x2 matrix from row-wise components
static Eigen::Matrix2d make_Matrix2d(const double& m00, const double& m01, const double& m10, const double& m11)
{
	Eigen::Matrix2d M;
	M << m00, m01, m10, m11;
	return M;
}

// Returns value of Binomial Coefficient C(n, k)
static int binomialCoeff(int n, int k)
{
	int res = 1;
	if (k > n - k)
		k = n - k;
	for (int i = 0; i < k; ++i) {
		res *= (n - i);
		res /= (i + 1);
	}
	return res;
}

static int factorial(int n)
{
	int ret = 1;
	for (int i = 1; i <= n; ++i)
		ret *= i;
	return ret;
}

static void ComputeDisplacementSystemMatrixCoefficients(int i, int j, const Eigen::Vector2d& xx, const Eigen::Vector2d& vv, const Eigen::Matrix2d& J, Eigen::Matrix2d& f, Eigen::Matrix2d& g)
{
	f = make_Matrix2d(0, 0, 0, 0), g = make_Matrix2d(0, 0, 0, 0);
	Eigen::Matrix2d I = make_Matrix2d(1, 0, 0, 1);
	double x = xx.x(), y = xx.y(), u = vv.x(), v = vv.y();

	double x_i = pow(x, i), x_i1 = pow(x, i - 1);
	double y_j = pow(y, j), y_j1 = pow(y, j - 1);

	int fac_i = factorial(i), fac_i1 = factorial(i - 1);
	int fac_j = factorial(j), fac_j1 = factorial(j - 1);

	f -= x_i * y_j / (fac_i * fac_j) * J;
	if (i != 0)
		f += u * x_i1*y_j / (fac_i1 * fac_j) * I;
	if (j != 0)
		f += v * x_i*y_j1 / (fac_i * fac_j1) * I;

	g = x_i * y_j / (fac_i * fac_j) * I;
}

vtkReferenceFrameFilter::vtkReferenceFrameFilter() : NeighborhoodU(10), Invariance(AffineInvariance), UseSummedAreaTables(true), TaylorOrder(2),
FieldNameV(NULL), FieldNameVx(NULL), FieldNameVy(NULL), FieldNameVt(NULL)
{
	SetFieldNameV("v");
	SetFieldNameVx("vx");
	SetFieldNameVy("vy");
	SetFieldNameVt("vt");
}

int vtkReferenceFrameFilter::RequestData(vtkInformation *vtkNotUsed(request),
	vtkInformationVector **inputVector,
	vtkInformationVector *outputVector)
{
	using namespace Eigen;

	// Get the info objects
	vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
	vtkInformation *outInfo = outputVector->GetInformationObject(0);

	// Get the input and ouptut
	vtkImageData *input = vtkImageData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));
	vtkImageData *output = vtkImageData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));

	// Get the information on the domain
	int* dims = input->GetDimensions();
	double* spacing = input->GetSpacing();
	double* boundsMin = input->GetOrigin();

	// select the system matrix size depending on the chosen invariance
	int systemSize = 6;
	switch (Invariance) {
	default:
	case Objectivity:			systemSize = 6;		break;
	case SimilarityInvariance:	systemSize = 8;		break;
	case AffineInvariance:		systemSize = 12;	break;
	case Displacement:			systemSize = 2 * TaylorOrder*TaylorOrder + TaylorOrder * 6 + 4; break;
	}

	// read the input data and abort if data is not present!
	float* input_v = NULL, *input_vx = NULL, *input_vy = NULL, *input_vt = NULL;
	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameV)) && input->GetPointData()->GetArray(FieldNameV)->GetNumberOfComponents() == 2)
		input_v = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameV))->GetPointer(0);
	else { cout << "Field " << FieldNameV << " was not found or does not have 2 components!" << endl; return 0; }

	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVx)) && input->GetPointData()->GetArray(FieldNameVx)->GetNumberOfComponents() == 2)
		input_vx = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVx))->GetPointer(0);
	else { cout << "Field " << FieldNameVx << " was not found or does not have 2 components!" << endl; return 0; }

	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVy)) && input->GetPointData()->GetArray(FieldNameVy)->GetNumberOfComponents() == 2)
		input_vy = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVy))->GetPointer(0);
	else { cout << "Field " << FieldNameVy << " was not found or does not have 2 components!" << endl; return 0; }

	if (vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVt)) && input->GetPointData()->GetArray(FieldNameVt)->GetNumberOfComponents() == 2)
		input_vt = vtkFloatArray::SafeDownCast(input->GetPointData()->GetArray(FieldNameVt))->GetPointer(0);
	else { cout << "Field " << FieldNameVt << " was not found or does not have 2 components!" << endl; return 0; }

	// Create an image to write the result to and copy the input into it (just for initialization)
	vtkSmartPointer<vtkImageData> image = vtkSmartPointer<vtkImageData>::New();
	image->DeepCopy(input);

	vtkDataArray* output_v = image->GetPointData()->GetArray(FieldNameV);
	vtkDataArray* output_vx = image->GetPointData()->GetArray(FieldNameVx);
	vtkDataArray* output_vy = image->GetPointData()->GetArray(FieldNameVy);
	vtkDataArray* output_vt = image->GetPointData()->GetArray(FieldNameVt);

	// Iterate the time steps (in parallel)
#ifdef NDEBUG
#pragma omp parallel for schedule(dynamic,16)
#endif
	for (int it = 0; it < dims[2]; it++)
	{
		// declare matrices for every voxel of a slice
		std::vector<MatrixXd, aligned_allocator<MatrixXd> > _M(dims[0] * dims[1]);
		std::vector<MatrixXd, aligned_allocator<MatrixXd> > _MTM(dims[0] * dims[1]);
		std::vector<MatrixXd, aligned_allocator<MatrixXd> > _MTb(dims[0] * dims[1]);

		// setup the system matrix M and right hand side.
		for (int iy = 0; iy < dims[1]; iy++)
		{
			double y = boundsMin[1] + iy * spacing[1];
			for (int ix = 0; ix < dims[0]; ix++)
			{
				double x = boundsMin[0] + ix * spacing[0];
				int tupleIdx = it * dims[0] * dims[1] + iy * dims[0] + ix;

				// position, velocity and derivatives at the voxel
				Vector2d xx(x, y);
				Vector2d vv(input_v[tupleIdx * 2 + 0], input_v[tupleIdx * 2 + 1]);
				Vector2d dx(input_vx[tupleIdx * 2 + 0], input_vx[tupleIdx * 2 + 1]);
				Vector2d dy(input_vy[tupleIdx * 2 + 0], input_vy[tupleIdx * 2 + 1]);
				Vector2d dt(input_vt[tupleIdx * 2 + 0], input_vt[tupleIdx * 2 + 1]);

				// compute 90 degree rotated vectors, setup Jacobian and compute products
				Vector2d Xp(-xx.y(), xx.x());
				Vector2d Vp(-vv.y(), vv.x());
				Matrix2d J = make_Matrix2d(dx, dy);
				Vector2d Jxpvp = -J * Xp + Vp;
				Vector2d Jxv = -J * xx + vv;

				// setup matrix M
				MatrixXd M(2, systemSize);
				switch (Invariance)
				{
				default:
				case Objectivity:
					M(0, 0) = Jxpvp.x(); 	 M(0, 1) = dx.x(); M(0, 2) = dy.x();   M(0, 3) = 1; M(0, 4) = 0;   M(0, 5) = Xp.x();
					M(1, 0) = Jxpvp.y(); 	 M(1, 1) = dx.y(); M(1, 2) = dy.y();   M(1, 3) = 0; M(1, 4) = 1;   M(1, 5) = Xp.y();
					break;
				case SimilarityInvariance:
					M(0, 0) = Jxpvp.x(); 	 M(0, 1) = dx.x(); M(0, 2) = dy.x();   M(0, 3) = 1; M(0, 4) = 0;   M(0, 5) = Xp.x(); M(0, 6) = Jxv.x(); M(0, 7) = xx.x();
					M(1, 0) = Jxpvp.y(); 	 M(1, 1) = dx.y(); M(1, 2) = dy.y();   M(1, 3) = 0; M(1, 4) = 1;   M(1, 5) = Xp.y(); M(1, 6) = Jxv.y();	M(1, 7) = xx.y();
					break;
				case AffineInvariance:
					M(0, 0) = vv.x() - xx.x() * J(0, 0);	M(0, 1) = 0 - xx.x() * J(0, 1);	/**/  M(0, 2) = vv.y() - xx.y() * J(0, 0);  M(0, 3) = 0 - xx.y() * J(0, 1);  /**/  M(0, 4) = J(0, 0);  M(0, 5) = J(0, 1);  /**/  M(0, 6) = 1;  M(0, 7) = 0;  /**/  M(0, 8) = xx.x();  M(0, 9) = 0;       /**/  M(0, 10) = xx.y();  M(0, 11) = 0;
					M(1, 0) = 0 - xx.x() * J(1, 0);	M(1, 1) = vv.x() - xx.x() * J(1, 1);	/**/  M(1, 2) = 0 - xx.y() * J(1, 0);  M(1, 3) = vv.y() - xx.y() * J(1, 1);  /**/  M(1, 4) = J(1, 0);  M(1, 5) = J(1, 1);  /**/  M(1, 6) = 0;	M(1, 7) = 1; /**/  M(1, 8) = 0;       M(1, 9) = xx.x();  /**/  M(1, 10) = 0;       M(1, 11) = xx.y();
					break;
				case Displacement:
				{
					int index = 0;
					for (int m = 0; m <= TaylorOrder; ++m)
					{
						for (int i = m; i >= 0; --i)
						{
							int j = m - i;
							Matrix2d f, g;
							ComputeDisplacementSystemMatrixCoefficients(i, j, xx, vv, J, f, g);
							M(0, index * 4 + 0) = f(0, 0);		M(1, index * 4 + 0) = f(1, 0);
							M(0, index * 4 + 1) = f(0, 1);		M(1, index * 4 + 1) = f(1, 1);

							M(0, index * 4 + 2) = g(0, 0);		M(1, index * 4 + 2) = g(1, 0);
							M(0, index * 4 + 3) = g(0, 1);		M(1, index * 4 + 3) = g(1, 1);
							index += 1;
						}
					}
					break;
				}
				}

				// store MTM and MTb
				MatrixXd MT = M.transpose();
				_M[iy*dims[0] + ix] = M;
				_MTM[iy*dims[0] + ix] = MT * M;
				_MTb[iy*dims[0] + ix] = MT * dt * ((Invariance == Displacement) ? -1 : 1);
			}
		}

		// compute the prefix sum
		if (UseSummedAreaTables)
		{
			for (int iy = 0; iy < dims[1]; iy++)
				for (int ix = 0; ix < dims[0]; ix++)
				{
					if (ix > 0) {
						_MTM[iy*dims[0] + ix] += _MTM[iy*dims[0] + (ix - 1)];
						_MTb[iy*dims[0] + ix] += _MTb[iy*dims[0] + (ix - 1)];
					}
					if (iy > 0) {
						_MTM[iy*dims[0] + ix] += _MTM[(iy - 1)*dims[0] + ix];
						_MTb[iy*dims[0] + ix] += _MTb[(iy - 1)*dims[0] + ix];
					}
					if (ix > 0 && iy > 0) {
						_MTM[iy*dims[0] + ix] -= _MTM[(iy - 1)*dims[0] + (ix - 1)];
						_MTb[iy*dims[0] + ix] -= _MTb[(iy - 1)*dims[0] + (ix - 1)];
					}
				}
		}

		// solve the system for each pixel
		for (int iy = 0; iy < dims[1]; iy++)
		{
			double y = boundsMin[1] + iy * spacing[1];
			for (int ix = 0; ix < dims[0]; ix++)
			{
				double x = boundsMin[0] + ix * spacing[0];
				int tupleIdx = it * dims[0] * dims[1] + iy * dims[0] + ix;

				// corner indices of the neighborhood region
				int x1 = std::min(std::max(0, ix - NeighborhoodU - 1), dims[0] - 1);
				int y1 = std::min(std::max(0, iy - NeighborhoodU - 1), dims[1] - 1);
				int x2 = std::min(std::max(0, ix + NeighborhoodU), dims[0] - 1);
				int y2 = std::min(std::max(0, iy + NeighborhoodU), dims[1] - 1);

				// compute the sum of all matrices/vectors in neighborhood region
				MatrixXd MTM(systemSize, systemSize);
				MTM.setZero();
				VectorXd MTb(systemSize, 1);
				MTb.setZero();

				if (UseSummedAreaTables)
				{
					MTM = _MTM[y2*dims[0] + x2] + _MTM[y1*dims[0] + x1] - _MTM[y1*dims[0] + x2] - _MTM[y2*dims[0] + x1];
					MTb = _MTb[y2*dims[0] + x2] + _MTb[y1*dims[0] + x1] - _MTb[y1*dims[0] + x2] - _MTb[y2*dims[0] + x1];
				}
				else
				{
					for (int wy = y1; wy <= y2; ++wy)
						for (int wx = x1; wx <= x2; ++wx)
						{
							MTM += _MTM[wy*dims[0] + wx];
							MTb += _MTb[wy*dims[0] + wx];
						}
				}

				// solve for reference frame parameters
				VectorXd uu = MTM.fullPivHouseholderQr().solve(MTb);

				// compute new vector field in optimal frame
				Vector2d xx(x, y);
				Vector2d vv(input_v[tupleIdx * 2 + 0], input_v[tupleIdx * 2 + 1]);
				Vector2d dx(input_vx[tupleIdx * 2 + 0], input_vx[tupleIdx * 2 + 1]);
				Vector2d dy(input_vy[tupleIdx * 2 + 0], input_vy[tupleIdx * 2 + 1]);
				Vector2d dt(input_vt[tupleIdx * 2 + 0], input_vt[tupleIdx * 2 + 1]);
				Matrix2d J = make_Matrix2d(dx, dy);
				Vector2d vnew(0, 0);
				Matrix2d Jnew = make_Matrix2d(0, 0, 0, 0);
				Vector2d Xp(-xx.y(), xx.x());
				switch (Invariance)
				{
				case Objectivity:
					vnew = vv + Vector2d(uu(1), uu(2)) - uu(0) * Xp;
					Jnew = J + make_Matrix2d(0, uu(0), -uu(0), 0);
					break;
				case SimilarityInvariance:
					vnew = vv + Vector2d(uu(1), uu(2)) - uu(0) * Xp - uu(6) * xx;
					Jnew = J + make_Matrix2d(0, uu(0), -uu(0), 0) - make_Matrix2d(uu(6), 0, 0, uu(6));
					break;
				case AffineInvariance:
				{
					Matrix2d H1 = make_Matrix2d(-uu(0), -uu(2), -uu(1), -uu(3));
					Vector2d k1(uu(4), uu(5));
					vnew = vv + H1 * xx + k1;
					Jnew = J + H1;
					break;
				}
				case Displacement:
				{
					// construct velocity
					{
						Vector2d Ft(0, 0);
						int fac_m = 1;
						for (int m = 0; m <= TaylorOrder; ++m)
						{
							if (m > 1)
								fac_m *= m;

							Vector2d Ftpart(0, 0);
							for (int i = 0; i <= m; ++i)
							{
								int fu_i = i, fu_j = m - i;
								// general formular to get the linear index:  (i+j)*(i+j+1) + 2*j
								int linear_fu = (fu_i + fu_j)*(fu_i + fu_j + 1) + 2 * fu_j;
								double fu = uu(2 * linear_fu + 0);
								double fv = uu(2 * linear_fu + 1);

								int binom = binomialCoeff(m, i);
								Ftpart += binom * pow(xx.x(), i) * pow(xx.y(), m - i) * Vector2d(fu, fv);
							}
							Ft += Ftpart / fac_m;
						}
						vnew = vv + Ft;
					}
					// construct Jacobian
					{
						Matrix2d nablaFt = make_Matrix2d(0, 0, 0, 0);
						int fac_m = 1;
						for (int m = 1; m <= TaylorOrder; ++m)
						{
							Matrix2d nablaFtpart = make_Matrix2d(0, 0, 0, 0);
							for (int i = 0; i <= m - 1; ++i)
							{
								int fx_i = i + 1, fx_j = m - 1 - i;
								int fy_i = i, fy_j = m - i;
								// general formular to get the linear index:  (i+j)*(i+j+1) + 2*j
								int linear_fx = (fx_i + fx_j)*(fx_i + fx_j + 1) + 2 * fx_j;
								int linear_fy = (fy_i + fy_j)*(fy_i + fy_j + 1) + 2 * fy_j;
								double fxu = uu(2 * linear_fx + 0);
								double fxv = uu(2 * linear_fx + 1);
								double fyu = uu(2 * linear_fy + 0);
								double fyv = uu(2 * linear_fy + 1);

								int binom = binomialCoeff(m - 1, i);
								nablaFtpart += binom * pow(xx.x(), i) * pow(xx.y(), m - 1 - i) * make_Matrix2d(fxu, fyu, fxv, fyv);
							}
							nablaFt += nablaFtpart / fac_m;

							if (m > 1)
								fac_m *= m;
						}
						Jnew = J + nablaFt;
					}
					break;
				}
				}
				Vector2d vtnew = dt - _M[iy * dims[0] + ix] * uu * ((Invariance == Displacement) ? -1 : 1);

				// store the result
				output_v->SetTuple2(tupleIdx, vnew.x(), vnew.y());
				output_vx->SetTuple2(tupleIdx, Jnew(0, 0), Jnew(1, 0));
				output_vy->SetTuple2(tupleIdx, Jnew(0, 1), Jnew(1, 1));
				output_vt->SetTuple2(tupleIdx, vtnew.x(), vtnew.y());
			}
		}
	}

	// Copy the computed image to the output
	output->ShallowCopy(image);

	// Update the extent
	int extent[6];
	input->GetExtent(extent);
	output->SetExtent(extent);
	outInfo->Set(vtkStreamingDemandDrivenPipeline::UPDATE_EXTENT(), extent, 6);
	outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(), extent, 6);
	return 1;
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      van der Ham, L. Interplanetary trajectory design using dynamical systems theory,
 *          MSc thesis, Delft University of Technology, Delft, The Netherlands, 2012.
 *      Mireles James, J.D. Celestial Mechanics Notes Set 4: The Circular Restricted Three Body
 *          Problem, 2006, http://www.math.utexas.edu/users/jjames/hw4Notes.pdf,
 *          last accessed: 18th May, 2012.
 *
 *    Notes
 *      WARNING: There seems to be a bug in the computation of the L3 location!
 *
 */

#include <cmath>
#include <stdexcept>

#include <boost/bind.hpp>

#include "Tudat/Astrodynamics/Gravitation/librationPoint.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace gravitation
{
namespace circular_restricted_three_body_problem
{

using namespace root_finders;
using namespace basic_mathematics;

//! Compute location of Lagrange libration point.
void LibrationPoint::computeLocationOfLibrationPoint(
        LagrangeLibrationPoints lagrangeLibrationPoint )
{
    using std::pow;
    using std::sqrt;

    // Set functions for Newton-Raphson based on collinear libration point passed as input
    // parameter, or computed locations directly of equilateral libration points.
    switch( lagrangeLibrationPoint )
    {
    case l1:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &LibrationPoint::computeL1LocationFunction, this, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &LibrationPoint::
                computeL1FirstDerivativeLocationFunction, this, std::placeholders::_1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, 1.0 ), 0.0, 0.0;
    }
        break;

    case l2:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &LibrationPoint::computeL2LocationFunction, this, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &LibrationPoint::
                computeL2FirstDerivativeLocationFunction, this, std::placeholders::_1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, 1.0 ), 0.0, 0.0;
    }
        break;

    case l3:
    {
        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &LibrationPoint::computeL3LocationFunction, this, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &LibrationPoint::
                computeL3FirstDerivativeLocationFunction, this, std::placeholders::_1 ) );

        // Set position vector of L1 in Cartesian elements based on result of Newton-Raphson
        // root-finding algorithm.
        positionOfLibrationPoint_ << rootFinder->execute( rootFunction, -1.0 ), 0.0, 0.0;
    }
        break;

    case l4:

        // Set position vector of L4 in Cartesian elements.
        positionOfLibrationPoint_.x( ) = 0.5 - massParameter;
        positionOfLibrationPoint_.y( ) = 0.5 * sqrt( 3.0 );
        positionOfLibrationPoint_.z( ) = 0.0;

        break;

    case l5:

        // Set position vector of L5 in Cartesian elements.
        positionOfLibrationPoint_.x( ) = 0.5 - massParameter;
        positionOfLibrationPoint_.y( ) = -0.5 * sqrt( 3.0 );
        positionOfLibrationPoint_.z( ) = 0.0;

        break;

    default:

        throw std::runtime_error(
                            "The Lagrange libration point requested does not exist." );
    };
}

} // namespace circular_restricted_three_body_problem
} // namespace gravitation
} // namespace tudat


#pragma once
#include <vector>
#include <set>
#include <Eigen/Dense>
#include <utility>
#include <ostream>
#include <map>
#include <atomic>
#include <mutex>
#include <span>

namespace ml
{

using namespace Eigen;

struct topology
{
    std::vector<int> layers;
};

std::ostream& operator<<(std::ostream& stream, const topology& topo);

class neural_network
{
private:
    std::vector<MatrixXd>                 m_weights;
    std::vector<VectorXd>                 m_biases;
    topology                              m_topo;
    const std::map<std::string, VectorXd> m_labels;
    std::mutex                            mtx;
    // Ideally m_latest_weight_gradients and m_latest_bias_gradients would not be properties of the class,
    // however back_propagate's function signature was getting too large
    std::vector<MatrixXd>                 m_latest_weight_gradients;
    std::vector<VectorXd>                 m_latest_bias_gradients;

    static void hadamard_product(VectorXd& lhs, const VectorXd& rhs);

    static void logistic(VectorXd& vector);
    static void logistic_derivative(VectorXd& vector);

    [[nodiscard]] static std::map<std::string, VectorXd> generate_class_vector_map(std::set<std::string>);

public:
    neural_network(topology topo, std::set<std::string> labels);
    neural_network(std::vector<MatrixXd> weights, std::vector<VectorXd> biases, std::set<std::string> labels);

    void randomize();

    // returns the largest activation of the final layer and it's coresponding class
    [[nodiscard]] std::pair<std::string, double>                classify(const VectorXd& input_activations) const;
    // Generates a confusion matrix with the columns representing predicted labels and the rows representing actual labels, vector component
    // of pair is to indicate ordering of labels in rows and columns
    [[nodiscard]] std::pair<std::vector<std::string>, MatrixXi> generate_confusion_matrix(const std::vector<std::pair<VectorXd, std::string>>& test_data) const;
    // returns a vector of all calculated activations and weighted sums
    [[nodiscard]] std::vector<std::pair<VectorXd, VectorXd>>    forward_propagate_return_all(const VectorXd& input_activations, const std::vector<MatrixXd>& local_weights, const std::vector<VectorXd>& local_biases) const;

    // performes back propagation. training_data is a vector of pairs of training input data and desired output classification
    double back_propagate(const std::vector<std::pair<VectorXd, VectorXd>>& training_data, const std::span<int>& mini_batch, double learning_rate);
    // trains network
    void   train(const std::vector<std::pair<VectorXd, std::string>>& training_data, bool print_cost, std::atomic_bool& stopped, double learning_rate = 0.1, std::size_t mini_batch_size = 0);

    friend std::ostream& operator<<(std::ostream&, const neural_network&);
};

std::ostream& operator<<(std::ostream& stream, const neural_network& network);

};


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <mimkl/definitions.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <stdexcept>

int main(int argc, char **argv)
{
    try
    {
        Eigen::Matrix<double, 2, 3> X;
        Eigen::SparseMatrix<double> L(3, 3);
        Eigen::Matrix<double, 2, 1> diag;
        Eigen::Matrix<double, 2, 1> diag_reference;

        X << 1., 2., 3., 4., 5., 6.;
        mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);

        diag_reference << 14., 77.;

        mimkl::induction::get_diagonal_from_square_induction(X, L, diag);
        assert(((diag - diag_reference).norm() == 0.0) && "Identity Inducer");

        Eigen::SparseMatrix<double> L1(3, 3);
        typedef Eigen::Triplet<double> TripletDouble; // (row,col,coef)
        std::vector<TripletDouble> triplet_list;
        triplet_list.reserve(4);
        triplet_list.push_back(TripletDouble(0, 1, 1.));
        triplet_list.push_back(TripletDouble(1, 2, 1.));
        triplet_list.push_back(TripletDouble(1, 0, 1.));
        triplet_list.push_back(TripletDouble(2, 1, 1.));
        L1.setFromTriplets(triplet_list.begin(), triplet_list.end());

        diag_reference << 16., 100.;

        mimkl::induction::get_diagonal_from_square_induction(X, L1, diag);
        assert(((diag - diag_reference).norm() == 0.0) &&
               "unweighted Graph Inducer");

        return EXIT_SUCCESS;
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what();
        return EXIT_FAILURE;
    }
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file knapsack_long_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_knapsack.hpp"
#include "test_utils/knapsack_tags_utils.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/dynamic/knapsack_unbounded.hpp"
#include "paal/dynamic/knapsack_0_1.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/algorithm/random_shuffle.hpp>

#include <fstream>

using namespace paal;
using namespace paal::utils;
using namespace paal::system;

BOOST_AUTO_TEST_CASE(KnapsackLong) {
    std::string test_dir = get_test_data_dir("KNAPSACK");
    parse(build_path(test_dir, "cases.txt"),
          [&](const std::string & line, std::istream & is_test_cases) {
        int testId = std::stoi(line);
        LOGLN("test >>>>>>>>>>>>>>>>>>>>>>>>>>>> " << testId);

        int capacity;
        std::vector<std::pair<int, int>> objects;
        std::vector<int> optimal;

        auto size = [](std::pair<int, int> object) { return object.first; }
        ;
        auto value = [](std::pair<int, int> object) { return object.second; }
        ;

        read(build_path(test_dir, "cases"), testId, capacity, objects, optimal);
        LOGLN("capacity " << capacity);
        LOGLN("sizes ");

        LOGLN("");
        LOGLN("size values");
        ON_LOG(for (auto o
                    : objects) {
            std::cout << "{ size = " << o.first << ", value = " << o.second
                      << "} ";
        });
        LOGLN("");
        LOGLN("Optimal 0/1");
        LOG_COPY_RANGE_DEL(optimal, " ");
        LOGLN("");
        auto opt_0_1 = boost::accumulate(optimal, 0, [&](int sum, int i) {
            return sum + objects[i].second;
        });
        ON_LOG(auto optSize = )
            boost::accumulate(optimal, 0, [&](int sum, int i) {
            return sum + objects[i].first;
        });
        LOGLN("Opt size " << optSize << " opt " << opt_0_1);
        LOGLN("");

        // KNAPSACK
        auto opt =
            detail_knapsack<pd::integral_value_and_size_tag, pd::unbounded_tag>(
                objects, capacity, size, value).first;

        boost::random_shuffle(objects);

        auto maxValue =
            detail_knapsack<pd::integral_value_tag, pd::unbounded_tag>(
                objects, capacity, size, value);

        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt, maxValue.first);
        maxValue = detail_knapsack<pd::integral_size_tag, pd::unbounded_tag>(
            objects, capacity, size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt, maxValue.first);

        // KNAPSACK 0/1

        maxValue =
            detail_knapsack<pd::integral_value_and_size_tag, pd::zero_one_tag>(
                objects, capacity, size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt_0_1, maxValue.first);
        maxValue = detail_knapsack<pd::integral_size_tag, pd::zero_one_tag>(
            objects, capacity, size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt_0_1, maxValue.first);
        maxValue = detail_knapsack<pd::integral_value_tag, pd::zero_one_tag>(
            objects, capacity, size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt_0_1, maxValue.first);

        maxValue =
            detail_knapsack<pd::integral_value_and_size_tag, pd::zero_one_tag,
                            pd::no_retrieve_solution_tag>(objects, capacity,
                                                          size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt_0_1, maxValue.first);
        maxValue = detail_knapsack<pd::integral_size_tag, pd::zero_one_tag,
                                   pd::no_retrieve_solution_tag>(
            objects, capacity, size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt_0_1, maxValue.first);
        maxValue = detail_knapsack<pd::integral_value_tag, pd::zero_one_tag,
                                   pd::no_retrieve_solution_tag>(
            objects, capacity, size, value);
        boost::random_shuffle(objects);
        BOOST_CHECK_EQUAL(opt_0_1, maxValue.first);

        // FPTAS
        auto epsilons = { 0.00001, 0.0001, 0.001, 0.01, 0.1, 0.2, 0.3, 0.4, 0.5,
                          0.6, 0.7 };

        for (auto epsilon : epsilons) {
            // KNAPSACK unbounded on value
            maxValue = detail_knapsack_fptas<pd::unbounded_tag,
                                             pd::retrieve_solution_tag>(
                epsilon, objects, capacity, size, value, on_value_tag{});
            boost::random_shuffle(objects);
            BOOST_CHECK(double(opt) * (1. - epsilon) <= maxValue.first);
            BOOST_CHECK(capacity >= maxValue.second);

            // KNAPSACK unbounded on size
            // this might possibly fail since this version is not really a FPTAS
            maxValue = detail_knapsack_fptas<pd::unbounded_tag,
                                             pd::retrieve_solution_tag>(
                epsilon, objects, capacity, size, value, on_size_tag{});
            boost::random_shuffle(objects);
            BOOST_CHECK(opt <= maxValue.first);
            BOOST_CHECK(double(capacity) * (1. + epsilon) >= maxValue.second);

            // KNAPSACK 0_1 on value
            maxValue = detail_knapsack_fptas<pd::zero_one_tag,
                                             pd::retrieve_solution_tag>(
                epsilon, objects, capacity, size, value, on_value_tag{});
            boost::random_shuffle(objects);
            BOOST_CHECK(double(opt_0_1) * (1. - epsilon) <= maxValue.first);
            BOOST_CHECK(capacity >= maxValue.second);

            // KNAPSACK 0_1 on size
            maxValue = detail_knapsack_fptas<pd::zero_one_tag,
                                             pd::retrieve_solution_tag>(
                epsilon, objects, capacity, size, value, on_size_tag{});
            boost::random_shuffle(objects);
            BOOST_CHECK(opt_0_1 <= maxValue.first);
            BOOST_CHECK(double(capacity) * (1. + epsilon) >= maxValue.second);
        }
    });
}