/* Copyright (c) 2018-2019 the `graphkernels` developers
 * All rights reserved.
 */

#include "rest.h"

#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>

#include <algorithm>
#include <utility>

using std::vector;
using std::pair;

using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::SparseMatrix;
using Eigen::VectorXd;

auto order_by_labels(const vector<int>& labels) {
    vector<pair<int, int>> map;
    map.reserve(labels.size());

    auto idx = 0;
    for (const auto label : labels) {
        map.emplace_back(label, idx++);
    }

    sort(map.begin(), map.end());
    return map;
}

auto compute_valid_vertex_pairs(
        const vector<pair<int, int>>& map1,
        const vector<pair<int, int>>& map2) {
    vector<pair<int, int>> pairs;
    pairs.reserve(map1.size() * map2.size());

    const auto comp = [](const auto& p_a, const auto& p_b){
        return p_a.first < p_b.first;
    };

    auto p = map2.cbegin();  // Memoize low limit (see below).
    for (auto i1 = map1.cbegin(); i1 != map1.cend(); ) {
        // Find range of map2 that contains vertices labelled "label1".
        auto [eq_cbegin, eq_cend] = std::equal_range(p, map2.cend(), *i1, comp);

        // Iterate over all equal values in map1.
        const auto label1 = i1->first;
        do {
            const auto num1 = i1->second;

            // Create all pairs between vertex of map1 and range of map2.
            for (auto p = eq_cbegin; p != eq_cend; ++p) {
                pairs.emplace_back(num1, p->second);
            }

            ++i1;
        } while (i1 != map1.cend() && i1->first == label1);

        // All vertices with that label have been exhausted in both maps.
        p = eq_cend;
    }

    sort(pairs.begin(), pairs.end());
    return pairs;
}

auto productAdjacency(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label) {
    // Step 1: Order vertices by labels; compute all valid vertex pairs
    const auto pairs = compute_valid_vertex_pairs(
            order_by_labels(v1_label),
            order_by_labels(v2_label));

    // Step 2: Compute new labels for vertices of the product graph.
    Eigen::Matrix<int, -1, -1, Eigen::RowMajor> H(v1_label.size(), v2_label.size());

    auto next_label = 0;
    for (const auto& [v1, v2] : pairs) {
        H(v1, v2) = next_label++;
    }

    // Step 3: Compute the adjacency matrix of the direct product graph.
    vector<Eigen::Triplet<double>> v;
    for (auto i = 0; i < e1.rows(); ++i) {
        const auto e1_s = e1(i, 0);
        const auto e1_t = e1(i, 1);
        const auto e1_label = e1(i, 2);

        for (auto j = 0; j < e2.rows(); ++j) {
            if (e1_label == e2(j, 2)) {
                const auto e2_s = e2(j, 0);
                const auto e2_t = e2(j, 1);

                if (v1_label[e1_s] == v2_label[e2_s]
                &&  v1_label[e1_t] == v2_label[e2_t]
                   ) {
                    v.emplace_back(H(e1_s, e2_s), H(e1_t, e2_t), 1.0);
                    v.emplace_back(H(e1_t, e2_t), H(e1_s, e2_s), 1.0);
                }

                if (v1_label[e1_s] == v2_label[e2_t]
                &&  v1_label[e1_t] == v2_label[e2_s]
                   ) {
                    v.emplace_back(H(e1_s, e2_t), H(e1_t, e2_s), 1.0);
                    v.emplace_back(H(e1_t, e2_s), H(e1_s, e2_t), 1.0);
                }
            }
        }
    }

    SparseMatrix<double> Ax(next_label, next_label);
    Ax.setFromTriplets(v.cbegin(), v.cend());

    return Ax;
}

double geometricRandomWalkKernel(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label,
        double lambda,
        int max_iterations,
        double eps) {
    // compute the adjacency matrix Ax of the direct product graph
    const SparseMatrix<double> Lx = lambda * productAdjacency(
            e1, e2, v1_label, v2_label);

    // inverse of I - lambda * Ax by fixed-poInt iterations
    const auto n_rows = Lx.rows();
    const VectorXd ones = VectorXd::Ones(n_rows);
    auto x = ones;
    VectorXd x_pre = VectorXd::Zero(n_rows);

    auto count = 0;
    do {
        x_pre = x;
        x = ones + Lx * x_pre;
        ++count;
    } while (count <= max_iterations && (x - x_pre).squaredNorm() > eps);
    return x.sum();
}

MatrixXd CalculateGeometricRandomWalkKernelPy(
        const vector<MatrixXi>& E,
        const vector<vector<int>>& V_label,
        double lambda,
        int max_iterations,
        double eps) {
    MatrixXd K(V_label.size(), V_label.size());

    for (auto j = 0; j < V_label.size(); ++j) {
        for (auto i = 0; i <= j; ++i) {
            K(i, j) = geometricRandomWalkKernel(
                    E[i], E[j], V_label[i], V_label[j], lambda,
                    max_iterations, eps);
        }
    }

    return K.selfadjointView<Eigen::Upper>();
}

double exponentialRandomWalkKernel(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label,
        double beta) {
    // compute the adjacency matrix Ax of the direct product graph
    const MatrixXd Ax = productAdjacency(e1, e2, v1_label, v2_label);

    return Ax.exp().sum();
}

MatrixXd CalculateExponentialRandomWalkKernelPy(
        const vector<MatrixXi>& E,
        const vector<vector<int>>& V_label,
        double beta) {
    MatrixXd K(V_label.size(), V_label.size());

    for (auto j = 0; j < V_label.size(); ++j) {
        for (auto i = 0; i <= j; ++i) {
            K(i, j) = exponentialRandomWalkKernel(
                    E[i], E[j], V_label[i], V_label[j], beta);
        }
    }

    return K.selfadjointView<Eigen::Upper>();
}

double kstepRandomWalkKernel(
        const MatrixXi& e1,
        const MatrixXi& e2,
        const vector<int>& v1_label,
        const vector<int>& v2_label,
        const vector<double>& lambda_list) {
    // compute the adjacency matrix Ax of the direct product graph
    const SparseMatrix<double> Ax = productAdjacency(e1, e2, v1_label, v2_label);

    // prepare identity matrix
    const auto n_rows = Ax.rows();
    SparseMatrix<double> I{n_rows, n_rows};
    I.setIdentity();

    auto Sum = SparseMatrix<double>{n_rows, n_rows};
    Sum.setZero();

    // Compute products until k using:
    // https://en.wikipedia.org/wiki/Horner%27s_method
    auto k = lambda_list.size();
    while (k-- > 0) {
        Sum = (Sum * Ax) + lambda_list[k] * I;
    }

    return Sum.sum();
}

MatrixXd CalculateKStepRandomWalkKernelPy(
        const vector<MatrixXi>& E,
        const vector<vector<int>>& V_label,
        const vector<double>& par) {
    MatrixXd K(V_label.size(), V_label.size());

    for (auto j = 0; j < V_label.size(); ++j) {
        for (auto i = 0; i <= j; ++i) {
            K(i, j) = kstepRandomWalkKernel(
                    E[i], E[j], V_label[i], V_label[j], par);
        }
    }

    return K.selfadjointView<Eigen::Upper>();
}


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include "GaussSeq.h"
#include "ARSeq.h"
#include "utils.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;
    using boost::random::uniform_real_distribution;

    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u (0, 0.25);

    // set precision of output
//    std::streamsize precision = std::numeric_limits<cpp_bin_float_50>::digits10;
//    std::cout << std::setprecision(10);

    // testing
    std::map<int, my_float> coeff {
        {1, -0.9},
    };
    ARSeq seq(coeff, 100);
    std::vector<my_float> v;
    for (auto i = 0; i < 1; i++) {
        v.push_back(u(gen));
    }

    seq.seed_prev_vals(v);

    seq.print_past_vals();
    for (auto i = 0; i < 100; i++) {
        seq.next();
    }
    std::cout << "After 100 iterations\n";
    seq.print_past_vals();

    for (auto i = 0; i < 900; i++) {
        seq.next();
    }
    std::cout << "After 1000 iterations\n";
    seq.print_past_vals();

    for (auto i = 0; i < 9000; i++) {
        seq.next();
    }
    std::cout << "After 10000 iterations\n";
    seq.print_past_vals();

    return 0;
}


/*    Copyright (c) 2010-2019, 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.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/math/quadrature/gaussianQuadrature.h"
#include "tudat/math/basic/mathematicalConstants.h"

namespace tudat
{
namespace unit_tests
{

// Functions to be tested

double sinFunction( const double x )
{
    return std::sin( x );
}

double expFunction( const double x )
{
    return std::exp( x );
}

double polyFunction( const double x )
{
    return std::pow( x, 9 ) + 2 * std::pow( x, 7 ) - std::pow( x, 4 ) + 8 * std::pow( x, 2 ) - 11;
}

//! Even-order derivatives for error assessment
double minSinFunction( const double x )
{
    return -std::sin( x );
}

std::function< double( const double ) > dSinFunction( unsigned int n )
{
    if ( n % 2 == 0 )
    {
        if ( n % 4 == 0 )
        {
            return sinFunction;
        }
        else
        {
            return minSinFunction;
        }
    }
    else
    {
        throw std::runtime_error( "Unknown derivative" );
    }
}

std::function< double( const double ) > dExpFunction( unsigned int n )
{
    return expFunction;
}

double d4PolyFunction( const double x )
{
    return 24 * ( 126 * std::pow( x, 5 ) + 70 * std::pow( x, 3 ) - 1 );
}

double d6PolyFunction( const double x )
{
    return 10080 * ( 6 * std::pow( x, 3 ) + x );
}

double d8PolyFunction( const double x )
{
    return 362800 * x;
}

std::function< double( const double ) > dPolyFunction( unsigned int n )
{
    switch( n )
    {
    case 4: return d4PolyFunction;
    case 6: return d6PolyFunction;
    case 8: return d8PolyFunction;
    default: throw std::runtime_error( "Unknown derivative" );
    }
}

// Factorial
unsigned int factorial( const unsigned int x )
{
    if ( x > 0 )
    {
        return x * factorial( x - 1 );
    }
    else
    {
        return 1;
    }
}

// Error function for Gaussian quadrature [ from https://en.wikipedia.org/wiki/Gaussian_quadrature#Error_estimates ]
double gaussianQuadratureError( const unsigned int n, std::function< double( const double ) > derivative2nth,
                                const double abscissa, const double lowerLimit, const double upperLimit )
{
    return std::pow( upperLimit - lowerLimit, 2 * n + 1 ) * std::pow( factorial( n ), 4 )
            / double( ( 2 * n + 1 ) * std::pow( factorial( 2 * n ), 3 ) ) * derivative2nth( abscissa );
}

// Check error is within bounds and decreasing for increasing number of nodes.
// Number of nodes ranges from minOrder to maxOrder (both included).
// It must hold that lowerLimit ≤ abscissa ≤ upperLimit.
// The derivative is a function taking as input an `int n` (the nth derivative) that returns a function taking as input
// a `double x` (the absicssa at which it will be evaluated).
void checkErrorWithinBounds( const unsigned int minOrder, const unsigned int maxOrder,
                             std::function< double( const double ) > function,
                             const double lowerLimit, const double upperLimit,
                             std::function< std::function< double( const double ) >( const unsigned int ) > derivative,
                             const double abscissa, const double expectedSolution )
{
    double obtainedError = TUDAT_NAN;
    double errorBound = TUDAT_NAN;
    for ( unsigned int n = minOrder; n <= maxOrder; n++ )
    {
        const double previousObtainedError = obtainedError;
        numerical_quadrature::GaussianQuadrature< double, double > integrator( function, lowerLimit, upperLimit, n );
        obtainedError = std::fabs( expectedSolution - integrator.getQuadrature() );
        errorBound = std::fabs( gaussianQuadratureError( n, derivative( 2 * n ), abscissa, lowerLimit, upperLimit ) );
        BOOST_CHECK( obtainedError < errorBound );
        if ( n > minOrder )
        {
            BOOST_CHECK( obtainedError < previousObtainedError );
        }
    }
}

BOOST_AUTO_TEST_SUITE( test_gaussian_quadrature )

//! Test if quadrature is computed correctly (sine function with 1E4 data points).
BOOST_AUTO_TEST_CASE( testIntegralSineFunction )
{
    using namespace mathematical_constants;
    using namespace numerical_quadrature;

    const unsigned int order = 8;
    const double lowerLimit = 0.0;
    const double upperLimit = PI;
    GaussianQuadrature< double, double > integrator( sinFunction, lowerLimit, upperLimit, order );
    double computedSolution = integrator.getQuadrature();

    // Expected solution from Wolfram Alpha
    double expectedSolution = 2.0;

    // Check if computed solution matches expected value for a high order (8).
    BOOST_CHECK_CLOSE_FRACTION( computedSolution, expectedSolution, 1E-10 );

    /// Check that it is a Gaussian quadrature and not just any quadrature

    // Check if error is within bounds for order 2...4
    checkErrorWithinBounds( 2, 4, sinFunction, lowerLimit, upperLimit, dSinFunction, PI / 2, expectedSolution );

    // Set to order 2
    integrator.reset( sinFunction, lowerLimit, upperLimit, 2 );
    computedSolution = integrator.getQuadrature();

    // Expected solution from http://keisan.casio.com/exec/system/1330940731
    expectedSolution = 1.9358195746511370184019497173109914411780661179299;

    // Check if computed solution matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( computedSolution, expectedSolution, 1E-12 );
}

//! Test if quadrature is computed correctly (exponential function).
BOOST_AUTO_TEST_CASE( testIntegralExpFunction )
{
    using namespace mathematical_constants;
    using namespace numerical_quadrature;

    const unsigned int order = 7;
    const double lowerLimit = -2.0;
    const double upperLimit = 2.0;
    GaussianQuadrature< double, double > integrator( expFunction, lowerLimit, upperLimit, order );
    double computedSolution = integrator.getQuadrature();

    // Expected solution from Wolfram Alpha
    double expectedSolution = 7.25372081569404;

    // Check if computed solution matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( computedSolution, expectedSolution, 1E-10 );

    /// Check that it is a Gaussian quadrature and not just any quadrature

    // Check if error is within bounds for order 2...6
    checkErrorWithinBounds( 2, 6, expFunction, lowerLimit, upperLimit, dExpFunction, 1.0, expectedSolution );

    // Set to order 2
    integrator.reset( expFunction, lowerLimit, upperLimit, 2 );
    computedSolution = integrator.getQuadrature();

    // Expected solution from http://keisan.casio.com/exec/system/1330940731
    expectedSolution = 6.9764499206151121988504219845072175547665355367817;

    // Check if computed solution matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( computedSolution, expectedSolution, 1E-12 );
}

//! Test if quadrature is computed correctly (polynomial function).
BOOST_AUTO_TEST_CASE( testIntegralPolyFunction )
{
    using namespace mathematical_constants;
    using namespace numerical_quadrature;

    // A polynomial of order 2*steps - 1 is computed exactly. Order of tested polynomial is 9, thus steps = 5.
    const unsigned int order = 5;
    const double lowerLimit = -2.0;
    const double upperLimit = 4.0;
    GaussianQuadrature< double, double > integrator( polyFunction, lowerLimit, upperLimit, order );
    double computedSolution = integrator.getQuadrature();

    // Expected solution from Wolfram Alpha
    double expectedSolution = 120990;

    // Check if computed solution matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( computedSolution, expectedSolution, 1E-12 );

    // Check that this isn't the case for 4 nodes
    integrator.reset( polyFunction, lowerLimit, upperLimit, 4 );
    computedSolution = integrator.getQuadrature();
    BOOST_CHECK( std::fabs( computedSolution - expectedSolution) > 1 );

    /// Check that it is a Gaussian quadrature and not just any quadrature

    // Check if error is within bounds for order 2...4
    checkErrorWithinBounds( 2, 4, polyFunction, lowerLimit, upperLimit, dPolyFunction, 2.0, expectedSolution );

    // Set to order 2
    integrator.reset( polyFunction, lowerLimit, upperLimit, 2 );
    computedSolution = integrator.getQuadrature();

    // Expected solution from http://keisan.casio.com/exec/system/1330940731
    expectedSolution = 32214;

    // Check if computed solution matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( computedSolution, expectedSolution, 1E-12 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Benoit Dequidt <benoit.dequidt@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <cstdlib>

#include <boost/compute/core.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/inclusive_scan.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/type_traits/type_name.hpp>
#include <boost/compute/utility/source.hpp>

namespace compute = boost::compute;

/// warning precision is not precise due
/// to the float error accumulation when size is large enough
/// for more precision use double
/// or a kahan sum else results can diverge
/// from the CPU implementation
compute::program make_sma_program(const compute::context& context)
{
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        __kernel void SMA(__global const float *scannedValues, int size, __global float *output, int wSize)
        {
            const int gid = get_global_id(0);

            float cumValues = 0.f;
            int endIdx = gid + wSize/2;
            int startIdx = gid -1 - wSize/2;

            if(endIdx > size -1)
                endIdx = size -1;

            cumValues += scannedValues[endIdx];
            if(startIdx < 0)
                startIdx = -1;
            else
                cumValues -= scannedValues[startIdx];

            output[gid] =(float)( cumValues / ( float )(endIdx - startIdx));
        }
   );

    // create sma program
    return compute::program::build_with_source(source,context);
}

bool check_results(const std::vector<float>& values, const std::vector<float>& smoothValues, unsigned int wSize)
{
    int size = values.size();
    if(size != (int)smoothValues.size()) return false;

    int semiWidth = wSize/2;

    bool res = true;
    for(int idx = 0 ; idx < size ; ++idx)
    {
        int start = (std::max)(idx - semiWidth,0);
        int end = (std::min)(idx + semiWidth,size-1);
        float res = 0;
        for(int j = start ; j <= end ; ++j)
        {
            res+= values[j];
        }

        res /= float(end - start +1);

        if(std::abs(res-smoothValues[idx]) > 1e-3)
        {
            std::cout << "idx = " << idx << " -- expected = " << res << " -- result = " << smoothValues[idx] << std::endl;
            res = false;
        }
    }

    return res;
}

// generate a uniform law over [0,10]
float myRand()
{
    static const double divisor = double(RAND_MAX)+1.;
    return double(rand())/divisor * 10.;
}

int main()
{
    unsigned int size = 1024;
    // wSize must be odd
    unsigned int wSize = 21;
    // get the default device
    compute::device device = compute::system::default_device();
    // create a context for the device
    compute::context context(device);
    // get the program
    compute::program program = make_sma_program(context);

    // create vector of random numbers on the host
    std::vector<float> host_vector(size);
    std::vector<float> host_result(size);
    std::generate(host_vector.begin(), host_vector.end(), myRand);

    compute::vector<float> a(size,context);
    compute::vector<float> b(size,context);
    compute::vector<float> c(size,context);
    compute::command_queue queue(context, device);

    compute::copy(host_vector.begin(),host_vector.end(),a.begin(),queue);

    // scan values
    compute::inclusive_scan(a.begin(),a.end(),b.begin(),queue);
    // sma kernel
    compute::kernel kernel(program, "SMA");
    kernel.set_arg(0,b.get_buffer());
    kernel.set_arg(1,(int)b.size());
    kernel.set_arg(2,c.get_buffer());
    kernel.set_arg(3,(int)wSize);

    using compute::uint_;
    uint_ tpb = 128;
    uint_ workSize = size;
    queue.enqueue_1d_range_kernel(kernel,0,workSize,tpb);

    compute::copy(c.begin(),c.end(),host_result.begin(),queue);

    bool res = check_results(host_vector,host_result,wSize);
    std::string status = res ? "results are equivalent" : "GPU results differs from CPU one's";
    std::cout << status << std::endl;

    return 0;
}


/*
 * Author: Patrick Schmidt
 */

#include "Helpers.hh"

#include <Eigen/SparseQR>

namespace HomologyInference
{

int rank(const SparseMatrix& _M)
{
    ISM_INFO("Starting QR factorization");

    Eigen::SparseQR<SparseMatrix, Eigen::COLAMDOrdering<int>> QR;
    QR.compute(_M);

    ISM_INFO("Finished QR factorization");

    const int r = QR.rank();

    ISM_ASSERT_EQ(_M.rows(), _M.cols());
    ISM_DEBUG_OUT("Matrix has rank n - " << _M.rows() - r);

    return r;
}

double total_area(const TriMesh &_mesh)
{
    double result = 0.0;
    for (auto fh : _mesh.faces())
        result += _mesh.calc_face_area(fh);

    return result;
}

std::pair<Vec3d, Vec3d> bounding_box(
        const TriMesh& _mesh)
{
    Vec3d pmin(+INF_DOUBLE, +INF_DOUBLE, +INF_DOUBLE);
    Vec3d pmax(-INF_DOUBLE, -INF_DOUBLE, -INF_DOUBLE);
    for (const auto& vh : _mesh.vertices())
    {
        const Vec3d& p = _mesh.point(vh);
        pmin = pmin.cwiseMin(p);
        pmax = pmax.cwiseMax(p);
    }
    return {pmin, pmax};
}

double bounding_box_diagonal(
        const TriMesh& _mesh)
{
    const auto [pmin, pmax] = bounding_box(_mesh);
    return (pmax - pmin).norm();
}

Color log_color(
        const double _val,
        const double _min,
        const double _max,
        const Color& _min_color,
        const Color& _max_color)
{
    ISM_ASSERT_G(_val, 0.0);

    double lambda = (std::log(_val) - std::log(_min)) / (std::log(_max) - std::log(_min));
    lambda = std::min(std::max(lambda, 0.0), 1.0);

    return (1.0 - lambda) * _min_color + lambda * _max_color;
}

bool incident(const TriMesh& _mesh, const VH _vh, const FH _fh)
{
    ISM_ASSERT(_mesh.is_valid_handle(_fh));
    for (const auto& vh : _mesh.fv_range(_fh))
        if (vh == _vh)
            return true;
    return false;
}

VH closest_vertex(
        const TriMesh& _mesh,
        const Vec3d& _p)
{
    double closest_dist = INF_DOUBLE;
    VH closest_vh;
    for (auto v : _mesh.vertices())
    {
        const double dist = (_mesh.point(v) - _p).norm();
        if (dist < closest_dist)
        {
            closest_dist = dist;
            closest_vh = v;
        }
    }

    return closest_vh;
}

}


//  Copyright John Maddock 2006.
//  Copyright Paul A. Bristow 2007.

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

#include <boost/math/tools/minima.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/gamma.hpp>

template <class T>
struct poly_test
{
   // minima is at (3,4):
   T operator()(const T& v)
   {
      T a = v - 3;
      return 3 * a * a + 4;
   }
};

template <class T>
void test_minima(T, const char* /* name */)
{
   std::pair<T, T> m = boost::math::tools::brent_find_minima(poly_test<T>(), T(-10), T(10), 50);
   BOOST_CHECK_CLOSE(m.first, T(3), T(0.001));
   BOOST_CHECK_CLOSE(m.second, T(4), T(0.001));

   T (*fp)(T);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   fp = boost::math::lgamma<T>;
#else
   fp = boost::math::lgamma;
#endif

   m = boost::math::tools::brent_find_minima(fp, T(0.5), T(10), 50);
   BOOST_CHECK_CLOSE(m.first, T(1.461632), T(0.1));
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   fp = boost::math::tgamma<T>;
#else
   fp = boost::math::tgamma;
#endif
   m = boost::math::tools::brent_find_minima(fp, T(0.5), T(10), 50);
   BOOST_CHECK_CLOSE(m.first, T(1.461632), T(0.1));
}

int test_main(int, char* [])
{
   test_minima(0.1f, "float");
   test_minima(0.1, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_minima(0.1L, "long double");
#endif
   return 0;
}


#include <string>
#include <iostream>
#include <fstream>
#include <math.h>
#include <vector>

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

using namespace std;

string scan_match_file = "./scan_match.txt";
string odom_file = "./odom.txt";

int main(int argc, char **argv)
{
    // 放置激光雷达的时间和匹配值 t_s s_x s_y s_th
    vector<vector<double>> s_data;
    // 放置轮速计的时间和左右轮角速度 t_r w_L w_R
    vector<vector<double>> r_data;

    ifstream fin_s(scan_match_file);
    ifstream fin_r(odom_file);
    if (!fin_s || !fin_r)
    {
        cerr << "请在有scan_match.txt和odom.txt的目录下运行此程序" << endl;
        return 1;
    }

    // 读取激光雷达的匹配值
    while (!fin_s.eof())
    {
        double s_t, s_x, s_y, s_th;
        fin_s >> s_t >> s_x >> s_y >> s_th;
        s_data.push_back(vector<double>({s_t, s_x, s_y, s_th}));
    }
    fin_s.close();

    // 读取两个轮子的角速度
    while (!fin_r.eof())
    {
        double t_r, w_L, w_R;
        fin_r >> t_r >> w_L >> w_R;
        r_data.push_back(vector<double>({t_r, w_L, w_R}));
    }
    fin_r.close();

    // 第一步：计算中间变量J_21和J_22
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    // 设置数据长度
    A.conservativeResize(5000, 2);
    b.conservativeResize(5000);
    A.setZero();
    b.setZero();

    size_t id_r = 0;
    size_t id_s = 0;
    double last_rt = r_data[0][0];
    double w_Lt = 0;
    double w_Rt = 0;
    while (id_s < 5000)
    {
        // 激光的匹配信息
        const double &s_t = s_data[id_s][0];
        const double &s_th = s_data[id_s][3];
        // 里程计信息
        const double &r_t = r_data[id_r][0];
        const double &w_L = r_data[id_r][1];
        const double &w_R = r_data[id_r][2];
        ++id_r;
        // 在2帧激光匹配时间内进行里程计角度积分
        if (r_t < s_t)
        {
            double dt = r_t - last_rt;
            w_Lt += w_L * dt;
            w_Rt += w_R * dt;
            last_rt = r_t;
        }
        else
        {
            double dt = s_t - last_rt;
            w_Lt += w_L * dt;
            w_Rt += w_R * dt;
            last_rt = s_t;
            // 填充A, b矩阵
            //TODO: (3~5 lines)
            A(id_s, 0) = w_Lt;
            A(id_s, 1) = w_Rt;
            b(id_s) = s_th;

            //end of TODO
            w_Lt = 0;
            w_Rt = 0;
            ++id_s;
        }
    }
    // 进行最小二乘求解
    Eigen::Vector2d J21J22;
    //TODO: (1~2 lines)
    J21J22 = A.householderQr().solve(b);

    //end of TODO
    const double &J21 = J21J22(0);
    const double &J22 = J21J22(1);
    cout << "J21: " << J21 << endl;
    cout << "J22: " << J22 << endl;

    // 第二步，求解轮间距b
    Eigen::VectorXd C;
    Eigen::VectorXd S;
    // 设置数据长度
    C.conservativeResize(10000);
    S.conservativeResize(10000);
    C.setZero();
    S.setZero();

    id_r = 0;
    id_s = 0;
    last_rt = r_data[0][0];
    double th = 0;
    double cx = 0;
    double cy = 0;
    while (id_s < 5000)
    {
        // 激光的匹配信息
        const double &s_t = s_data[id_s][0];
        const double &s_x = s_data[id_s][1];
        const double &s_y = s_data[id_s][2];
        // 里程计信息
        const double &r_t = r_data[id_r][0];
        const double &w_L = r_data[id_r][1];
        const double &w_R = r_data[id_r][2];
        ++id_r;
        // 在2帧激光匹配时间内进行里程计位置积分
        if (r_t < s_t)
        {
            double dt = r_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
            th += (J21 * w_L + J22 * w_R) * dt;
            last_rt = r_t;
        }
        else
        {
            double dt = s_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);
            th += (J21 * w_L + J22 * w_R) * dt;
            last_rt = s_t;
            // 填充C, S矩阵
            //TODO: (4~5 lines)
            C(2 * id_s) = cx;
            C(2 * id_s + 1) = cy;
            S(2 * id_s) = s_x;
            S(2 * id_s + 1) = s_y;

            //end of TODO
            cx = 0;
            cy = 0;
            th = 0;
            ++id_s;
        }
    }
    // 进行最小二乘求解，计算b, r_L, r_R
    double b_wheel;
    double r_L;
    double r_R;
    //TODO: (3~5 lines)
    b_wheel = C.householderQr().solve(S)(0);
    r_L = -b_wheel * J21J22(0);
    r_R = b_wheel * J21J22(1);

    //end of TODO
    cout << "b: " << b_wheel << endl;
    cout << "r_L: " << r_L << endl;
    cout << "r_R: " << r_R << endl;

    cout << "参考答案：轮间距b为0.6m左右，两轮半径为0.1m左右" << endl;

    return 0;
}


// Cpush+ version 0.10
// defines a class for particle tracks
// in a uniform magnetic field, with
// a simple leapfrog pushing method.

#include <iostream>
#include <fstream>
#include <time.h>

#include <string>
#include <cmath>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

typedef Vector3d eigvec;
typedef Matrix3d eigmat;

const double pi = 3.1415926;
const eigmat I  = eigmat::Identity();

// particle track class
class track{
public:
  // initialise track properties
  double mass, charge;

  // object constructor
  track(string);

  // field interpolation method
  eigmat interpolate(eigvec x, double step){
    // retrieve vector field
    eigvec B_ = eigvec(0,0,1);

    // compute magnetic rotation map
    eigmat hatmap;
    hatmap << +0,     -B_[2], +B_[1],
              +B_[2], +0,     -B_[0],
              -B_[1], +B_[0], +0;
    hatmap *= (charge * step) / (2 * mass);
    return hatmap;
  }

  // leapfrog pushing method
  int leapfrog(double step, int nsteps){
    // set transverse velocity
    double eV = 1E-03;
    double vx = sqrt((2 * eV * abs(charge))/mass);

    // initialise dynamic variables
    eigvec x_ = eigvec(0,  0, 0);
    eigvec v_ = eigvec(vx, 0, 0);

    // backwards half-step the velocity
    eigmat hatmap = -(1./2.) * interpolate(x_, step);
    v_  = (I + hatmap).inverse() * (I - hatmap) * v_;

    // open a stream to write to file
    fstream track_io ("my_track.bin", ios::out | ios::binary);
    if (track_io.is_open()){

      // particle pushing loop
      for (int n = 0; n <= nsteps; n++){

        // write the position & velocity to file
        track_io << n << ' ' << ' ' << x_[0] << ' ' << x_[1] << ' ' << x_[2];
        track_io      << ' ' << ' ' << v_[0] << ' ' << v_[1] << ' ' << v_[2] << endl;

        // update the velocity and position
        hatmap = interpolate(x_, step);
        v_  = (I + hatmap).inverse() * (I - hatmap) * v_;
        x_ += v_ * step;
      }
    }
    return 0;
  }
};

// track object constructor
track::track(string ptype){
  if ((ptype == "antiproton") || (ptype == "pbar")){
    mass   = +1.67E-27;
    charge = -1.60E-19;
  }

  if ((ptype == "positron") || (ptype == "e+")){
    mass   = +9.11E-31;
    charge = +1.60-19;
  }
}

int main(){
  // example of the class
  track my_track ("pbar");
  double step = 6.56E-09;
  my_track.leapfrog(step, 10000);
}


// STL includes
#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

using namespace std;

int dijkstra_dist(const weighted_graph &G, int s, int t) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));

  return dist_map[t];
}

void solve()
{
  int n; cin >> n;
  int m; cin >> m;
  int s; cin >> s;
  int a; cin >> a;
  int b; cin >> b;

  vector<weighted_graph> graphs(s, weighted_graph(n));
  vector<weight_map> weights(s);
  for (int i = 0; i < s; ++i) {
    weights[i] = boost::get(boost::edge_weight, graphs[i]);
  }

  int u, v, w;
  for (int i = 0; i < m; ++i) {
    cin >> u;
    cin >> v;
    for (int j = 0; j < s; ++j) {
      cin >> w;
      boost::add_edge(u, v, w, graphs[j]);
    }
  }

  int h;
  for (int i = 0; i < s; ++i) {
    cin >> h;
  }

  weighted_graph G(n);

  for (int i = 0; i < s; ++i) {
    vector<edge_desc> mst;
    boost::kruskal_minimum_spanning_tree(graphs[i], back_inserter(mst));
    for (auto e : mst) {
      int u = boost::source(e, graphs[i]);
      int v = boost::target(e, graphs[i]);
      boost::add_edge(u, v, weights[i][e], G);
    }
  }

  cout << dijkstra_dist(G, a, b) << endl;

}

int main() {
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


#include <iostream>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, int>> weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor vertex_desc;

using namespace std;

int dijkstra_dist(const weighted_graph &G, int s) {
  int n = boost::num_vertices(G);
  std::vector<int> dist_map(n);

  boost::dijkstra_shortest_paths(G, s,
    boost::distance_map(boost::make_iterator_property_map(
      dist_map.begin(), boost::get(boost::vertex_index, G))));

  int value = 0;
  for (int i = 0; i < n; ++i) {
    if (dist_map[i] != 2147483647)
    value = max(value, dist_map[i]);
  }

  return value;
}

void solve() {
  int n; cin >> n;
  int m; cin >> m;

  weighted_graph G(4);
  weight_map weights = boost::get(boost::edge_weight, G);

  int u, v, w;
  for (int i = 0; i < m; ++i) {
    cin >> u;
    cin >> v;
    cin >> w;
    boost::add_edge(u, v, w, G);
  }

  vector<edge_desc> mst;    // vector to store MST edges (not a property map!)

  boost::kruskal_minimum_spanning_tree(G, back_inserter(mst));

  int weight = 0;
  for (vector<edge_desc>::iterator it = mst.begin(); it != mst.end(); ++it) {
    weight += boost::get(boost::edge_weight_t(), G, *it);
  }

  cout << weight << " " << dijkstra_dist(G, 0) << endl;
}

int main() {
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


//
//  quadratic_polynomial.cpp
//  BGV-Adder
//
//  Created by Andris on 27/10/2020.
//  Copyright © 2020 RUG. All rights reserved.
//

#include "quadratic_polynomial.hpp"
#include <helib/FHE.h>
#include <NTL/ZZX.h>
#include <NTL/tools.h>

using helib::Ctxt;

long quadratic_polynomial(int16_t aVal, int16_t bVal, int16_t cVal, int16_t xVal) {
    long k = 128; // Security parameter
    long L = 128; // Number of levels in the modulus default is 16
    long c = 3; // Nr of columns in key switch matrix.
    long w = 64; // secret key hamming weight

    // Change to 65537 for noise warning. (Always fun times)
    // Compensate for that by setting L = 128. (Increase modchain basically)
    // Set P to 65537 and L to 32 to showcase decryption failures.

    long p = 65537; // plaintext base default = 1021
    long d = 0; // Degree of field extension
    long r = 1; // hensel lifting

    // Determine a value for m
    auto m = helib::FindM(k, L, c, p, d, 0, 0);
    // Setup context
    auto context = helib::Context(m, p, r);
    // Build mod chain
    helib::buildModChain(context, L, c);

    // Generating secret key and public key
    NTL::ZZX encryption_polynomial = context.alMod.getFactorsOverZZ()[0];
    auto secretKey = helib::SecKey(context);
    secretKey.GenSecKey();
    const helib::PubKey& publicKey = secretKey;

    // Initialize ciphertexts
    Ctxt aVal_ciphertext = Ctxt(publicKey);
    Ctxt bVal_ciphertext = Ctxt(publicKey);
    Ctxt cVal_ciphertext = Ctxt(publicKey);
    Ctxt xVal_ciphertext = Ctxt(publicKey);

    // Plaintext must be encrypted as a polynomial using zzx api.
    publicKey.Encrypt(aVal_ciphertext, NTL::ZZX(aVal));
    publicKey.Encrypt(bVal_ciphertext, NTL::ZZX(bVal));
    publicKey.Encrypt(cVal_ciphertext, NTL::ZZX(cVal));
    publicKey.Encrypt(xVal_ciphertext, NTL::ZZX(xVal));

    // Apply operations on the ciphertexts.

    aVal_ciphertext *= xVal_ciphertext;
    aVal_ciphertext *= xVal_ciphertext;
    bVal_ciphertext *= xVal_ciphertext;

    aVal_ciphertext += bVal_ciphertext;
    aVal_ciphertext += cVal_ciphertext;

    Ctxt cipherResult = aVal_ciphertext;

    // Decrypt the results using secret key and convert back from
    // polynomial representation to numeric.
    long return_value = 0;
    NTL::ZZX plaintext_result;
    NTL::ZZX zzx;
    secretKey.Decrypt(zzx, cipherResult);
    conv(return_value, zzx[0]);
    if (return_value > p / 2) {
        return_value += (-1 * p);
    }

    return return_value;
}


#include <iostream>
#include <math.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Core>

#include "../include/Filter.h"

#include "../include/Robot.h"
#include "../include/Odom.h"
#include "../include/Imu.h"
#include "../include/Gps.h"

WeightingFilter weightingFilter;
WeightingFilter* pweightingFilter;

KalmanFilter kalmanFilter;
KalmanFilter *pkalmanFilter;

// Constructor
KalmanFilter::KalmanFilter()
{
    is_initialized_ = false;
    lastTimeStamp_ = 0;
    nowTimeStamp_ = 0;
    deltaTime_ = 0;
}

//Destructor
KalmanFilter::~KalmanFilter()
{
    delete pkalmanFilter;
}

void KalmanFilter::Initialization()
{
    lastTimeStamp_ = nowTimeStamp_ = getSysTime();

    Eigen::VectorXf x_in(5,1);
    x_in << 0,
            0,
            0,
            0,
            0;
    SetX(x_in);

    // state covariance matrix the prediction error
    Eigen::MatrixXf P_in(5,5);
    P_in << 0.1, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.1, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.1, 0.0, 0.0,
            0.0, 0.0, 0.0, 0.1, 0.0,
            0.0, 0.0, 0.0, 0.0, 0.1;
    SetP(P_in);

    //process covariance matrix
    Eigen::MatrixXf Q_in(5,5);
    Q_in << 0.1, 0.0, 0.0, 0.0, 0.0,
            0.0, 0.1, 0.0, 0.0, 0.0,
            0.0, 0.0, 0.1, 0.0, 0.0,
            0.0, 0.0, 0.0, 0.1, 0.0,
            0.0, 0.0, 0.0, 0.0, 0.1;
    SetQ(Q_in);

    //Observation
    Eigen::VectorXf z_in(3,1);
    z_in << 0.0,
            0.0,
            0.0;

    //measurement matrix
    Eigen::MatrixXf H_in(3,5);
    H_in << 0.0, 0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 0.0, 1.0,
            0.0, 0.0, 0.0, 0.0, 1.0;
    SetH(H_in);

    //measurement covariance matrix
    // R is provided by Sensor supplier
    Eigen::MatrixXf R_in(3,3);
    R_in << 0.1, 0.0, 0.0,
            0.0, 0.1, 0.0,
            0.0, 0.0, 0.1;
    SetR(R_in);

    is_initialized_ = true;

}

void KalmanFilter::SetX(Eigen::VectorXf x_in)
{
    x_ = x_in;
}

bool KalmanFilter::GetIsInitialized()
{
    return is_initialized_;
}

void KalmanFilter::SetF(Eigen::MatrixXf F_in)
{
    F_ = F_in;
}

void KalmanFilter::SetP(Eigen::MatrixXf P_in)
{
    P_ = P_in;
}

void KalmanFilter::SetQ(Eigen::MatrixXf Q_in)
{
    Q_ = Q_in;
}

void KalmanFilter::SetH(Eigen::MatrixXf H_in)
{
    H_ = H_in;
}

void KalmanFilter::SetR(Eigen::MatrixXf R_in)
{
    R_ = R_in;
}

void KalmanFilter::Prediction(float d_t) //预测
{
    float x_k, y_k, theta_k, v_k, w_k;
    x_k = x_[0];
    y_k = x_[1];
    theta_k = x_[2];
    v_k = x_[3];
    w_k = x_[4];

    x_ << x_k + v_k*d_t*cos(theta_k),y_k + v_k*d_t*sin(theta_k), theta_k + w_k*d_t, v_k,  w_k;
    Eigen::MatrixXf F_in(5,5);
    F_in << 1, 0, 0, d_t*cos(x_[2]), 0,
            0, 1, 0, d_t*sin(x_[2]), 0,
            0, 0, 1, 0, d_t,
            0, 0, 0, 1, 0,
            0, 0, 0, 0, 1;

    kalmanFilter.SetF(F_in);
    Eigen::MatrixXf Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
}
/*
void KalmanFilter::CalculateJacobianMatrix()
{
    Eigen::MatrixXf Hj(4,4);

    //get state paraeters
    float px = x_(0);
    float py = x_(1);
    float vx = x_(2);
    float vy = x_(3);

    //pre-compute a set of terms to avoid repeated calculation
    Hj << 1.0, 0.0, 0.0, 0.0,   //GPS 与 ODOM与IMU加权平均 后的结果进行再次融合
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 0.0, 0.0,
          0.0, 0.0, 0.0, 0.0;

    SetH(Hj);
}
*/
void KalmanFilter::KFUpdate(const Eigen::VectorXf &z)
{
    int size = x_.size();
    std::cout << size << std::endl;
    Eigen::VectorXf y = z - H_ * x_;
    Eigen::MatrixXf S = H_ * P_ * H_.transpose() + R_;
    Eigen::MatrixXf K = P_ * H_.transpose() * S.inverse();
    x_ = x_ + (K * y);
    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(size, size);
    P_ = (I - K * H_) * P_;
    //std::cout << "x_:" << std::endl << x_ << std::endl;

}
/*
void KalmanFilter::EKFUpdate(const Eigen::VectorXf &z)
{

    // Eigen::VectorXf h = Eigen::VectorXf(4);
    CalculateJacobianMatrix();

    Eigen::VectorXf y = z - H_ * x_;
    Eigen::MatrixXf Ht = H_.transpose();
    Eigen::MatrixXf S = H_ * P_ * Ht + R_;
    Eigen::MatrixXf Si = S.inverse();
    Eigen::MatrixXf K = P_ * Ht * Si;

    x_ = x_ + (K * y);

    int x_size = x_.size();
    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
	std::cout << "x_:" << std::endl << x_ <<std::endl;

}
*/
Eigen::VectorXf KalmanFilter::GetX()
{
    return x_;
}

void KalmanFilter::setLastTimeStamp(const long int lastTimeStamp)
{
    this->lastTimeStamp_ = lastTimeStamp;
}

void KalmanFilter::setNowTimeStamp(const long int nowTimeStamp )
{
    this->nowTimeStamp_ = nowTimeStamp;
}
void KalmanFilter::setDeltaTime(const long int deltaTime)
{
    this->deltaTime_ = deltaTime;
}

void KalmanFilter::getLastTimeStamp(long int& lastTimeStamp)
{
    lastTimeStamp = this->lastTimeStamp_;
}
void KalmanFilter::getNowTimeStamp(long int& nowTimeStamp)
{
    nowTimeStamp = this->nowTimeStamp_;
}
void KalmanFilter::getDeltaTime(long int& deltaTime)
{
    deltaTime = this->deltaTime_;
}

WeightingFilter::WeightingFilter()
{

}

WeightingFilter::~WeightingFilter()
{
    delete pweightingFilter;
}

void WeightingFilter::WeightingFilterUpdate()  //只是进行位置融合
{
    float odom_x, odom_y, odom_theta;
    float imu_x, imu_y, imu_z, imu_theta;
    float Roll; //翻滚角
    float Pitch; //俯仰角
    float Yaw; //偏航角

    odom.GetPos(odom_x, odom_y, odom_theta);
    imu.GetPosition( imu_x, imu_y, imu_theta);

    float robot_x, robot_y, robot_theta;

    robot_x = WEIGHT * odom_x + (1- WEIGHT) * imu_x;
    robot_y = WEIGHT * odom_y + (1- WEIGHT) * imu_y;
    imu.GetPostureYPR(Roll, Pitch, Yaw);
    robot_theta = Yaw/57.3;

    robot.SetRobotPosition(robot_x, robot_y, 0);
    robot.SetRobotRotation(Roll, Pitch, Yaw);
    odom.SetPos(robot_x,robot_y,robot_theta);
    imu.SetPosition(robot_x,robot_y,robot_theta);
}


//
// Created by Alex Beccaro on 27/12/18.
//

#include "problem111.hpp"
#include <primes.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using primes::is_prime;
using generics::from_digits;
using generics::combinations_repetition;
using std::vector;
using std::string;

using boost::multiprecision::uint128_t;

namespace problems {
    uint64_t problem111::primes_sum_from_bitmask(const std::string bitmask, uint32_t digit) {
        uint64_t n = bitmask.size(), m = 0;
        vector<uint32_t> digits(n);

        for (uint32_t i = 0; i < n; ++i) {
            if (bitmask[i]) {
                digits[i] = digit;
                m++;
            } else
                digits[i] = digit + 1;
        }

        vector<uint32_t> all = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
        all.erase(std::remove(all.begin(), all.end(), digit), all.end());
        auto combs = combinations_repetition(all, n - m);

        uint64_t sum = 0;
        for (const auto &c : combs) {
            vector<uint32_t> digs(digits);
            uint32_t j = 0;

            for (uint32_t i = 0; i < digs.size(); i++)
                if (digs[i] != digit)
                    digs[i] = c[j++];

            uint64_t num = from_digits<uint64_t>(digs);
            if (digs[0] != 0 && is_prime<uint128_t>(num))
                sum += num;
        }

        return sum;
    }

    uint64_t problem111::solve(uint32_t n) {
        uint64_t res = 0;

        for (uint32_t d = 0; d < 10; d++) {
            uint32_t m = n;
            uint64_t sum = 0;
            bool found_m = false;

            while (!found_m) {
                m--;

                string bitmask(m, 1); // 1 -> d
                bitmask.resize(n, 0); // 0 -> !d

                do {
                    uint64_t s = primes_sum_from_bitmask(bitmask, d);

                    if (s > 0) {
                        found_m = true;
                        sum += s;
                    }
                } while (std::prev_permutation(bitmask.begin(), bitmask.end()));
            }

            res += sum;
        }

        return res;
    }
}


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

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

/**
* \file     gamma_distribution.cpp
* \author   Collin Johnson
*
* Implementation of GammaDistribution.
*/

#include <math/gamma_distribution.h>
#include <math/roots.h>
#include <boost/math/special_functions/gamma.hpp>
#include <algorithm>
#include <functional>
#include <iostream>

namespace vulcan
{
namespace math
{

double gamma_normalizer(double k, double theta) { return 1.0 / (boost::math::tgamma(k) * std::pow(theta, k)); }

GammaDistribution::GammaDistribution(double k, double theta)
: k_(k)
, theta_(theta)
{
    assert(k_ > 0.0);
    assert(theta_ > 0.0);

    normalizer_ = gamma_normalizer(k_, theta_);
}

double GammaDistribution::sample(void) const
{
    std::cout << "STUB: GammaDistribution::sample(void)\n";
    return 0.0;
}

double GammaDistribution::likelihood(double value) const
{
    return normalizer_ * std::pow(value, k_-1.0) * std::exp(-value / theta_);
}

bool GammaDistribution::save(std::ostream& out) const
{
    out << k_ << ' ' << theta_ << '\n';
    return out.good();
}

bool GammaDistribution::load(std::istream& in)
{
    in >> k_ >> theta_;

    assert(k_ > 0.0);
    assert(theta_ > 0.0);

    normalizer_ = gamma_normalizer(k_, theta_);

    return in.good();
}

}
}


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

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/with_error.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<double, stats<tag::error_of<tag::mean(lazy)> > > acc;
    acc(1.1);
    acc(1.2);
    acc(1.3);
    BOOST_CHECK_CLOSE(0.057735, accumulators::error_of<tag::mean(lazy)>(acc), 1e-4);

    accumulator_set<double, stats<tag::error_of<tag::mean(immediate)> > > acc2;
    acc2(1.1);
    acc2(1.2);
    acc2(1.3);
    BOOST_CHECK_CLOSE(0.057735, accumulators::error_of<tag::mean(immediate)>(acc2), 1e-4);

    accumulator_set<double, stats<with_error<tag::mean(lazy)> > > acc3;
    acc3(1.1);
    acc3(1.2);
    acc3(1.3);
    BOOST_CHECK_CLOSE(0.057735, accumulators::error_of<tag::mean(lazy)>(acc3), 1e-4);

    accumulator_set<double, stats<with_error<tag::mean(immediate)> > > acc4;
    acc4(1.1);
    acc4(1.2);
    acc4(1.3);
    BOOST_CHECK_CLOSE(0.057735, accumulators::error_of<tag::mean(immediate)>(acc4), 1e-4);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("mean test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/
#ifndef KINDR_LINEARALGEBRA_LINEARALGEBRA_HPP_
#define KINDR_LINEARALGEBRA_LINEARALGEBRA_HPP_

#include <Eigen/SVD>

namespace kindr {
//! Linear algebra methods
namespace linear_algebra {

/*!
 * \brief Gets a skew-symmetric matrix from a (column) vector
 * \param   vec 3x1-matrix (column vector)
 * \return skew   3x3-matrix
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 3> getSkewMatrixFromVector(const Eigen::Matrix<PrimType_, 3, 1>& vec) {
  Eigen::Matrix<PrimType_, 3, 3> mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

/*!
 * \brief Gets a 3x1 vector from a skew-symmetric matrix
 * \param   matrix 3x3-matrix
 * \return  column vector (3x1-matrix)
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 1> getVectorFromSkewMatrix(const Eigen::Matrix<PrimType_, 3, 3>& matrix) {
  return Eigen::Matrix<PrimType_, 3, 1> (matrix(2,1), matrix(0,2), matrix(1,0));
}

/*!
 * \brief Computes the Moore–Penrose pseudoinverse
 * info: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
 * \param a: Matrix to invert
 * \param result: Result is written here
 * \param epsilon: Numerical precision (for example 1e-6)
 * \return true if successful
 */
template<typename _Matrix_Type_>
bool static pseudoInverse(const _Matrix_Type_ &a, _Matrix_Type_ &result, double epsilon = std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon())
{
  Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

  typename _Matrix_Type_::Scalar tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV() * _Matrix_Type_( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() * svd.matrixU().adjoint();

  return true;
}

} // end namespace linear_algebra
} // end namespace kindr

#endif /* KINDR_LINEARALGEBRA_LINEARALGEBRA_HPP_ */


/**
 * @file pfasst/quadrature.hpp
 * @since v0.1.0
 */
#ifndef _PFASST__QUADRATURE_HPP_
#define _PFASST__QUADRATURE_HPP_

#include <cmath>
#include <exception>
#include <vector>
using namespace std;

#include <Eigen/Dense>
template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

#include "pfasst/config.hpp"
#include "pfasst/interfaces.hpp"
#include "pfasst/quadrature/polynomial.hpp"
#include "pfasst/quadrature/interface.hpp"
#include "pfasst/quadrature/gauss_lobatto.hpp"
#include "pfasst/quadrature/gauss_legendre.hpp"
#include "pfasst/quadrature/gauss_radau.hpp"
#include "pfasst/quadrature/clenshaw_curtis.hpp"
#include "pfasst/quadrature/uniform.hpp"

template<typename scalar>
using Matrix = Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

namespace pfasst
{
  /**
   * Functionality related to computing quadrature nodes and weights.
   *
   * @note Please note, that all quadrature nodes are in the range \\( [0, 1] \\).
   */
  namespace quadrature
  {
    /**
     * Instantiates quadrature handler for given number of nodes and type descriptor.
     *
     * @tparam precision numerical type of the nodes (e.g. `double`)
     * @param[in] nnodes number of quadrature nodes
     * @param[in] qtype type descriptor of the quadrature
     * @returns instance of pfasst::quadrature::IQuadrature of specified type with desired number
     *   of nodes
     * @throws pfasst::ValueError if @p qtype is not a valid quadrature type descriptor
     */
    template<typename precision = pfasst::time_precision>
    shared_ptr<IQuadrature<precision>> quadrature_factory(const size_t nnodes,
                                                          const QuadratureType qtype)
    {
      if (qtype == QuadratureType::GaussLegendre) {
        return make_shared<GaussLegendre<precision>>(nnodes);
      } else if (qtype == QuadratureType::GaussLobatto) {
        return make_shared<GaussLobatto<precision>>(nnodes);
      } else if (qtype == QuadratureType::GaussRadau) {
        return make_shared<GaussRadau<precision>>(nnodes);
      } else if (qtype == QuadratureType::ClenshawCurtis) {
        return make_shared<ClenshawCurtis<precision>>(nnodes);
      } else if (qtype == QuadratureType::Uniform) {
        return make_shared<Uniform<precision>>(nnodes);
      } else {
        throw ValueError("invalid node type passed to compute_nodes.");
        return nullptr;
      }
    }

    /**
     * Compute quadrature nodes for given quadrature type descriptor.
     *
     * @tparam precision numerical type of the nodes (e.g. `double`)
     * @param[in] nnodes number of quadrature nodes to compute
     * @param[in] qtype type descriptor of the quadrature nodes
     * @returns std::vector of quadrature nodes of given type
     *
     * @see pfasst::quadrature::QuadratureType for valid types
     * @see pfasst::quadrature::quadrature_factory for further details
     */
    template<typename precision = pfasst::time_precision>
    vector<precision> compute_nodes(size_t nnodes, QuadratureType qtype)
    {
      return quadrature_factory<precision>(nnodes, qtype)->get_nodes();
    }

    /**
     * Compute weights to interpolate from @p src nodes to @p dst nodes.
     *
     * @tparam precision numerical type of the interpolation (e.g. `double`)
     */
    template<typename precision = time_precision>
    Matrix<precision> compute_interp(vector<precision> dst, vector<precision> src)
    {
      const size_t ndst = dst.size();
      const size_t nsrc = src.size();

      Matrix<precision> mat(ndst, nsrc);

      for (size_t i = 0; i < ndst; i++) {
        for (size_t j = 0; j < nsrc; j++) {
          precision den = 1.0;
          precision num = 1.0;

          for (size_t k = 0; k < nsrc; k++) {
            if (k == j) { continue; }
            den *= src[j] - src[k];
            num *= dst[i] - src[k];
          }

          if (abs(num) > 1e-32) {
            mat(i, j) = num / den;
          } else {
            mat(i, j) = 0.0;
          }
        }
      }

      return mat;
    }
  }  // ::pfasst::quadrature
}  // ::pfasst

#endif  // _PFASST__QUADRATURE_HPP_


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/functional.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto fact = fix(
    [](auto fact, auto n) {
        return eval_if(n == int_<0>,
            always(int_<1>),
            [=](auto id) { return n * fact(n - id(int_<1>)); }
        );
    }
);

constexpr unsigned long long reference(unsigned long long n)
{ return n == 0 ? 1 : n * reference(n - 1); }

template <int n>
constexpr void test() {
    BOOST_HANA_CONSTANT_ASSERT(fact(ullong<n>) == ullong<reference(n)>);
    test<n - 1>();
}

template <> constexpr void test<-1>() { }

int main() {
    test<15>();
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE MomentumTest
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// RBDyn
#include "RBDyn/CoM.h"
#include "RBDyn/EulerIntegration.h"
#include "RBDyn/FA.h"
#include "RBDyn/FK.h"
#include "RBDyn/FV.h"
#include "RBDyn/Momentum.h"
#include "RBDyn/MultiBody.h"
#include "RBDyn/MultiBodyConfig.h"

// arm
#include "XYZSarm.h"

const double TOL = 1e-6;

BOOST_AUTO_TEST_CASE(centroidalMomentum)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm();

  VectorXd q(mb.nrParams());
  VectorXd alpha(mb.nrDof());
  CentroidalMomentumMatrix cmm(mb);

  {
    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);
    rbd::paramToVector(mbc.alpha, alpha);

    Vector3d com = rbd::computeCoM(mb, mbc);
    ForceVecd momentum = rbd::computeCentroidalMomentum(mb, mbc, com);
    cmm.computeMatrix(mb, mbc, com);

    ForceVecd momentumM(cmm.matrix() * alpha);

    BOOST_CHECK_EQUAL(momentum.vector().norm(), 0.);
    BOOST_CHECK_EQUAL(momentumM.vector().norm(), 0.);
  }

  // test J·q against computeCentroidalMomentum
  for(int i = 0; i < 100; ++i)
  {
    q.setRandom();
    q.segment<4>(mb.jointPosInParam(mb.jointIndexByName("j3"))).normalize();
    alpha.setRandom();
    rbd::vectorToParam(q, mbc.q);
    rbd::vectorToParam(alpha, mbc.alpha);

    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);

    Vector3d com = rbd::computeCoM(mb, mbc);
    ForceVecd momentum = rbd::computeCentroidalMomentum(mb, mbc, com);
    cmm.computeMatrix(mb, mbc, com);

    ForceVecd momentumM(cmm.matrix() * alpha);

    BOOST_CHECK_SMALL((momentum - momentumM).vector().norm(), TOL);
  }

  // test J·q against CentroidalMomentumMatrix::momentum
  for(int i = 0; i < 50; ++i)
  {
    std::vector<double> weight(mb.nrBodies());
    for(std::size_t i = 0; i < weight.size(); ++i)
    {
      weight[i] = Eigen::Matrix<double, 1, 1>::Random()(0);
    }

    CentroidalMomentumMatrix cmmW(mb, weight);

    q.setRandom();
    q.segment<4>(mb.jointPosInParam(mb.jointIndexByName("j3"))).normalize();
    alpha.setRandom();
    rbd::vectorToParam(q, mbc.q);
    rbd::vectorToParam(alpha, mbc.alpha);

    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);

    Vector3d com = rbd::computeCoM(mb, mbc);
    ForceVecd momentum = cmmW.momentum(mb, mbc, com);
    cmmW.computeMatrix(mb, mbc, com);

    ForceVecd momentumM(cmmW.matrix() * alpha);

    BOOST_CHECK_SMALL((momentum - momentumM).vector().norm(), TOL);
  }
}

BOOST_AUTO_TEST_CASE(centroidalMomentumDot)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm();

  CentroidalMomentumMatrix cmm(mb);

  VectorXd q(mb.nrParams());
  VectorXd alpha(mb.nrDof());
  VectorXd alphaD(mb.nrDof());

  for(int i = 0; i < 10; ++i)
  {
    q.setRandom();
    q.segment<4>(mb.jointPosInParam(mb.jointIndexByName("j3"))).normalize();
    alpha.setRandom();
    alphaD.setRandom();
    rbd::vectorToParam(q, mbc.q);
    rbd::vectorToParam(alpha, mbc.alpha);
    rbd::vectorToParam(alphaD, mbc.alphaD);

    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);
    rbd::forwardAcceleration(mb, mbc);

    for(int j = 0; j < 10; ++j)
    {
      Vector3d oldCom = rbd::computeCoM(mb, mbc);
      ForceVecd oldMomentum = rbd::computeCentroidalMomentum(mb, mbc, oldCom);
      Vector3d oldComVel = rbd::computeCoMVelocity(mb, mbc);

      ForceVecd momentumDot = rbd::computeCentroidalMomentumDot(mb, mbc, oldCom, oldComVel);
      cmm.computeMatrixAndMatrixDot(mb, mbc, oldCom, oldComVel);
      MatrixXd cmmMatrix = cmm.matrix();
      MatrixXd cmmMatrixDot = cmm.matrixDot();
      Vector6d momentumDotCMM = cmmMatrix * alphaD + cmmMatrixDot * alpha;

      // check that the momentum are the same
      BOOST_CHECK_SMALL((momentumDot.vector() - momentumDotCMM).norm(), TOL);

      // check that each compute compute the same thing
      cmm.computeMatrix(mb, mbc, oldCom);
      cmm.computeMatrixDot(mb, mbc, oldCom, oldComVel);

      BOOST_CHECK_SMALL((cmmMatrix - cmm.matrix()).norm(), TOL);
      BOOST_CHECK_SMALL((cmmMatrixDot - cmm.matrixDot()).norm(), TOL);

      rbd::eulerIntegration(mb, mbc, 1e-8);

      rbd::forwardKinematics(mb, mbc);
      rbd::forwardVelocity(mb, mbc);
      rbd::forwardAcceleration(mb, mbc);

      rbd::paramToVector(mbc.alpha, alpha);
      rbd::paramToVector(mbc.alphaD, alphaD);

      Vector3d newCom = rbd::computeCoM(mb, mbc);
      ForceVecd newMomentum = rbd::computeCentroidalMomentum(mb, mbc, newCom);
      ForceVecd momentumDotDiff = (newMomentum - oldMomentum) * (1. / 1e-8);

      BOOST_CHECK_SMALL((momentumDot - momentumDotDiff).vector().norm(), TOL);
    }
  }

  // test JDot·q against CentroidalMomentumMatrix::normalMomentumDot
  for(int i = 0; i < 50; ++i)
  {
    std::vector<double> weight(mb.nrBodies());
    for(std::size_t i = 0; i < weight.size(); ++i)
    {
      weight[i] = Eigen::Matrix<double, 1, 1>::Random()(0);
    }

    CentroidalMomentumMatrix cmmW(mb, weight);

    q.setRandom();
    q.segment<4>(mb.jointPosInParam(mb.jointIndexByName("j3"))).normalize();
    alpha.setRandom();
    alphaD.setZero();
    rbd::vectorToParam(q, mbc.q);
    rbd::vectorToParam(alpha, mbc.alpha);
    // calcul the normal acceleration since alphaD is zero
    rbd::vectorToParam(alphaD, mbc.alphaD);

    rbd::forwardKinematics(mb, mbc);
    rbd::forwardVelocity(mb, mbc);
    rbd::forwardAcceleration(mb, mbc);

    Vector3d com = rbd::computeCoM(mb, mbc);
    Vector3d comDot = rbd::computeCoMVelocity(mb, mbc);
    ForceVecd normalMomentumDot1 = cmmW.normalMomentumDot(mb, mbc, com, comDot);
    ForceVecd normalMomentumDot2 = cmmW.normalMomentumDot(mb, mbc, com, comDot, mbc.bodyAccB);
    cmmW.computeMatrixDot(mb, mbc, com, comDot);

    ForceVecd normalMomentumDotM(cmmW.matrixDot() * alpha);

    BOOST_CHECK_SMALL((normalMomentumDot1 - normalMomentumDotM).vector().norm(), TOL);
    BOOST_CHECK_SMALL((normalMomentumDot2 - normalMomentumDotM).vector().norm(), TOL);
  }
}


#include <stan/math/prim.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>

using Eigen::VectorXd;
using stan::math::softmax;

TEST(ProbDistributionsCategoricalLogit, error_check) {
  using stan::math::categorical_logit_rng;
  boost::random::mt19937 rng;

  VectorXd beta(3);

  beta << 1.0, 10.0, -10.0;
  EXPECT_NO_THROW(categorical_logit_rng(beta, rng));

  beta << -1e3, 1.1e3, 1e5;
  EXPECT_NO_THROW(categorical_logit_rng(beta, rng));

  beta(1) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(categorical_logit_rng(beta, rng), std::domain_error);

  beta(1) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(categorical_logit_rng(beta, rng), std::domain_error);
}

TEST(ProbDistributionsCategoricalLogit, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = 3;
  VectorXd beta(K);

  beta << -0.5, 0.1, 0.3;

  VectorXd theta = softmax(beta);
  boost::math::chi_squared mydist(K - 1);

  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * theta(i);
  }

  for (int i = 0; i < N; i++) {
    int a = stan::math::categorical_logit_rng(beta, rng);
    bin[a - 1]++;
  }

  double chi = 0;
  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


/**
 * \file boost/numeric/ublasx/operation/cat.hpp
 *
 * \brief Concatenate arrays along specified dimension.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompwhiching file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \todo cat<1>, cat<2>, cat<tag::major>, ...
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLAS_OPERATION_EX_CAT_HPP
#define BOOST_NUMERIC_UBLAS_OPERATION_EX_CAT_HPP

#include <algorithm>
#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>

//TODO: add overloaded function to cat vectors and vector-matrix pairs

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

template <
	typename M1,
	typename M2
>
struct matrix_cat_traits
{
	typedef typename promote_traits<
			typename matrix_traits<M1>::value_type,
			typename matrix_traits<M2>::value_type
		>::promote_type value_type;
	// Currently, the result type is simply a dense matrix since it is difficult
	// to know in advance whether a particular matrix structure will be
	// preserved after the 'cat' operation.
	// TODO: we might use the matrix_temporary_traits. However, how do we choose
	// between matrix_temporary_traits<M1> and matrix_temporary_traits<M2>?
	typedef matrix<
			value_type,
			typename layout_type<M1>::type
		> result_type;
};

/**
 * \brief Concatenate arrays along columns.
 *
 * \tparam InMatrixExpr1T The type of the first input matrix expression.
 * \tparam InMatrixExpr2T The type of the second input matrix expression.
 *
 * \param A The first input matrix expression.
 * \param B The second input matrix expression.
 * \return A new matrix with \c max(num_rows(A),num_rows(B)) rows and
 *  \c num_columns(A)+num_columns(B) columns.
 *
 * For two input matrices A and B, append each column of B to its respective
 * column of A.
 * If \a A and \a B have a different number of columns, the number of colums of
 * the resulting matrix will be the maximum between the number of columns of the
 * two input matrices and the elements of the matrix with the smaller number of
 * columns will be replaced with a zero value.
 *
 * Examples:
 *

 * A = [1 2 3;
 *      4 5 6;
 *      7 8 9]
 * B = [10 11;
 *      12 13]
 * C = cat_columns(A,B);
 * C == [ 1  2 3;
 *        4  5 6;
 *        7  8 9;
 *       10 11 0;
 *       12 13 0]
 *

 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <
	typename InMatrixExpr1T,
	typename InMatrixExpr2T
>
typename matrix_cat_traits<InMatrixExpr1T,InMatrixExpr2T>::result_type cat_columns(matrix_expression<InMatrixExpr1T> const& A, matrix_expression<InMatrixExpr2T> const& B)
{
	typedef typename matrix_cat_traits<InMatrixExpr1T,InMatrixExpr2T>::result_type out_matrix_type;
	typedef typename matrix_traits<out_matrix_type>::value_type value_type;
	typedef typename matrix_traits<out_matrix_type>::size_type size_type;

//	// precondition: num_columns(A) == num_columns(B)
//	BOOST_UBLAS_CHECK(
//		num_columns(A) == num_columns(B),
//		bad_argument()
//	);

	size_type A_nr = num_rows(A);
	size_type A_nc = num_columns(A);
	size_type B_nr = num_rows(B);
	size_type B_nc = num_columns(B);
	size_type nc = ::std::max(A_nc, B_nc);

	out_matrix_type X(A_nr+B_nr, nc, value_type());

	for (size_type c = 0; c < nc; ++c)
	{
		if (c < A_nc)
		{
			for (size_type r = 0; r < A_nr; ++r)
			{
				X(r,c) = A()(r,c);
			}
		}
		if (c < B_nc)
		{
			for (size_type r = 0; r < B_nr; ++r)
			{
				X(r+A_nr,c) = B()(r,c);
			}
		}
	}

	return X;
}

/**
 * \brief Concatenate arrays along rows.
 *
 * \tparam InMatrixExpr1T The type of the first input matrix expression.
 * \tparam InMatrixExpr2T The type of the second input matrix expression.
 *
 * \param A The first input matrix expression.
 * \param B The second input matrix expression.
 * \return A new matrix with \c num_rows(A)+num_rows(B) rows and
 *  \c max(num_columns(A),num_columns(B)) columns.
 *
 * For two input matrices A and B, append each column of B to its respective
 * column of A.
 * If \a A and \a B have a different number of columns, the number of rows of
 * the resulting matrix will be the maximum between the number of rows of the
 * two input matrices and the elements of the matrix with the smaller number of
 * columns will be replaced with a zero value.
 *
 * Examples:
 *

 * A = [1 2 3;
 *      4 5 6;
 *      7 8 9]
 * B = [10 11;
 *      12 13]
 * C = cat_rows(A,B);
 * C == [1 2 3 10 11;
 *       4 5 6 12 13;
 *       7 8 9  0  0];
 *

 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <
	typename InMatrixExpr1T,
	typename InMatrixExpr2T
>
typename matrix_cat_traits<InMatrixExpr1T,InMatrixExpr2T>::result_type cat_rows(matrix_expression<InMatrixExpr1T> const& A, matrix_expression<InMatrixExpr2T> const& B)
{
	typedef typename matrix_cat_traits<InMatrixExpr1T,InMatrixExpr2T>::result_type out_matrix_type;
	typedef typename matrix_traits<out_matrix_type>::value_type value_type;
	typedef typename matrix_traits<out_matrix_type>::size_type size_type;

	size_type A_nr = num_rows(A);
	size_type A_nc = num_columns(A);
	size_type B_nr = num_rows(B);
	size_type B_nc = num_columns(B);
	size_type nr = ::std::max(A_nr, B_nr);

	out_matrix_type X(nr, A_nc+B_nc, value_type());

	for (size_type r = 0; r < nr; ++r)
	{
		if (r < A_nr)
		{
			for (size_type c = 0; c < A_nc; ++c)
			{
				X(r,c) = A()(r,c);
			}
		}
		if (r < B_nr)
		{
			for (size_type c = 0; c < B_nc; ++c)
			{
				X(r,c+A_nc) = B()(r,c);
			}
		}
	}

	return X;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_CAT_HPP


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    Array<int,2> A(3,3);
    A = 3, 5, 8,
        2, 4, 9,
        1, 0, 1;

    // workaround for MIPSpro compiler bug
#if defined(__sgi)
    Array<int,2> Asqrd(3,3);
    Asqrd = sqr(A);
#endif

    TinyVector<int,2> minpos = minIndex(A);

    BZTEST(minpos(0) == 2);
    BZTEST(minpos(1) == 1);

    // workaround for MIPSpro compiler bug
#if defined(__sgi)
    minpos = minIndex(Asqrd);
#else
    minpos = minIndex(sqr(A));
#endif
    BZTEST(minpos(0) == 2);
    BZTEST(minpos(1) == 1);

    TinyVector<int,2> maxpos = maxIndex(A);
    BZTEST(maxpos(0) == 1);
    BZTEST(maxpos(1) == 2);

    // workaround for MIPSpro compiler bug
#if defined(__sgi)
    maxpos = maxIndex(Asqrd);
#else
    maxpos = maxIndex(sqr(A));
#endif
    BZTEST(maxpos(0) == 1);
    BZTEST(maxpos(1) == 2);
}


// Copyright (C) 2010  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/statistics.h>
#include <dlib/rand.h>
#include <algorithm>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.statistics");

    class statistics_tester : public tester
    {
    public:
        statistics_tester (
        ) :
            tester ("test_statistics",
                    "Runs tests on the statistics component.")
        {}

        void test_random_subset_selector ()
        {
            random_subset_selector<double> rand_set;

            for (int j = 0; j < 30; ++j)
            {
                print_spinner();

                running_stats<double> rs, rs2;

                rand_set.set_max_size(1000);

                for (double i = 0; i < 100000; ++i)
                {
                    rs.add(i);
                    rand_set.add(i);
                }

                for (unsigned long i = 0; i < rand_set.size(); ++i)
                    rs2.add(rand_set[i]);

                dlog << LDEBUG << "true mean:    " << rs.mean();
                dlog << LDEBUG << "true sampled: " << rs2.mean();
                double ratio = rs.mean()/rs2.mean();
                DLIB_TEST_MSG(0.96 < ratio  && ratio < 1.04, " ratio: " << ratio);
            }

            {
                random_subset_selector<int> r1, r2;
                r1.set_max_size(300);
                for (int i = 0; i < 4000; ++i)
                    r1.add(i);

                ostringstream sout;
                serialize(r1, sout);
                istringstream sin(sout.str());
                deserialize(r2, sin);

                DLIB_TEST(r1.size() == r2.size());
                DLIB_TEST(r1.max_size() == r2.max_size());
                DLIB_TEST(r1.next_add_accepts() == r2.next_add_accepts());
                DLIB_TEST(std::equal(r1.begin(), r1.end(), r2.begin()));

                for (int i = 0; i < 4000; ++i)
                {
                    r1.add(i);
                    r2.add(i);
                }

                DLIB_TEST(r1.size() == r2.size());
                DLIB_TEST(r1.max_size() == r2.max_size());
                DLIB_TEST(r1.next_add_accepts() == r2.next_add_accepts());
                DLIB_TEST(std::equal(r1.begin(), r1.end(), r2.begin()));
            }
        }

        void test_random_subset_selector2 ()
        {
            random_subset_selector<double> rand_set;
            DLIB_TEST(rand_set.next_add_accepts() == false);
            DLIB_TEST(rand_set.size() == 0);
            DLIB_TEST(rand_set.max_size() == 0);

            for (int j = 0; j < 30; ++j)
            {
                print_spinner();

                running_stats<double> rs, rs2;

                rand_set.set_max_size(1000);
                DLIB_TEST(rand_set.next_add_accepts() == true);

                for (double i = 0; i < 100000; ++i)
                {
                    rs.add(i);
                    if (rand_set.next_add_accepts())
                        rand_set.add(i);
                    else
                        rand_set.add();
                }

                DLIB_TEST(rand_set.size() == 1000);
                DLIB_TEST(rand_set.max_size() == 1000);

                for (unsigned long i = 0; i < rand_set.size(); ++i)
                    rs2.add(rand_set[i]);

                dlog << LDEBUG << "true mean:    " << rs.mean();
                dlog << LDEBUG << "true sampled: " << rs2.mean();
                double ratio = rs.mean()/rs2.mean();
                DLIB_TEST_MSG(0.96 < ratio  && ratio < 1.04, " ratio: " << ratio);
            }
        }

        void test_running_covariance (
        )
        {
            dlib::rand rnd;
            std::vector<matrix<double,0,1> > vects;

            running_covariance<matrix<double,0,1> > cov, cov2;
            DLIB_TEST(cov.in_vector_size() == 0);

            for (unsigned long dims = 1; dims < 5; ++dims)
            {
                for (unsigned long samps = 2; samps < 10; ++samps)
                {
                    vects.clear();
                    cov.clear();
                    DLIB_TEST(cov.in_vector_size() == 0);
                    for (unsigned long i = 0; i < samps; ++i)
                    {
                        vects.push_back(randm(dims,1,rnd));
                        cov.add(vects.back());

                    }
                    DLIB_TEST(cov.in_vector_size() == (long)dims);

                    DLIB_TEST(equal(mean(vector_to_matrix(vects)), cov.mean()));
                    DLIB_TEST_MSG(equal(covariance(vector_to_matrix(vects)), cov.covariance()),
                              max(abs(covariance(vector_to_matrix(vects)) - cov.covariance()))
                              << "   dims = " << dims << "   samps = " << samps
                              );
                }
            }

            for (unsigned long dims = 1; dims < 5; ++dims)
            {
                for (unsigned long samps = 2; samps < 10; ++samps)
                {
                    vects.clear();
                    cov.clear();
                    cov2.clear();
                    DLIB_TEST(cov.in_vector_size() == 0);
                    for (unsigned long i = 0; i < samps; ++i)
                    {
                        vects.push_back(randm(dims,1,rnd));
                        if ((i%2) == 0)
                            cov.add(vects.back());
                        else
                            cov2.add(vects.back());

                    }
                    DLIB_TEST((cov+cov2).in_vector_size() == (long)dims);

                    DLIB_TEST(equal(mean(vector_to_matrix(vects)), (cov+cov2).mean()));
                    DLIB_TEST_MSG(equal(covariance(vector_to_matrix(vects)), (cov+cov2).covariance()),
                              max(abs(covariance(vector_to_matrix(vects)) - (cov+cov2).covariance()))
                              << "   dims = " << dims << "   samps = " << samps
                              );
                }
            }

        }

        void test_running_stats()
        {
            print_spinner();

            running_stats<double> rs, rs2;

            running_scalar_covariance<double> rsc1, rsc2;

            for (double i = 0; i < 100; ++i)
            {
                rs.add(i);

                rsc1.add(i,i);
                rsc2.add(i,i);
                rsc2.add(i,-i);
            }

            // make sure the running_stats and running_scalar_covariance agree
            DLIB_TEST_MSG(std::abs(rs.mean() - rsc1.mean_x()) < 1e-10, std::abs(rs.mean() - rsc1.mean_x()));
            DLIB_TEST(std::abs(rs.mean() - rsc1.mean_y()) < 1e-10);
            DLIB_TEST(std::abs(rs.stddev() - rsc1.stddev_x()) < 1e-10);
            DLIB_TEST(std::abs(rs.stddev() - rsc1.stddev_y()) < 1e-10);
            DLIB_TEST(std::abs(rs.variance() - rsc1.variance_x()) < 1e-10);
            DLIB_TEST(std::abs(rs.variance() - rsc1.variance_y()) < 1e-10);
            DLIB_TEST(rs.current_n() == rsc1.current_n());

            DLIB_TEST(std::abs(rsc1.correlation() - 1) < 1e-10);
            DLIB_TEST(std::abs(rsc2.correlation() - 0) < 1e-10);

            // test serialization of running_stats
            ostringstream sout;
            serialize(rs, sout);
            istringstream sin(sout.str());
            deserialize(rs2, sin);
            // make sure the running_stats and running_scalar_covariance agree
            DLIB_TEST_MSG(std::abs(rs2.mean() - rsc1.mean_x()) < 1e-10, std::abs(rs2.mean() - rsc1.mean_x()));
            DLIB_TEST(std::abs(rs2.mean() - rsc1.mean_y()) < 1e-10);
            DLIB_TEST(std::abs(rs2.stddev() - rsc1.stddev_x()) < 1e-10);
            DLIB_TEST(std::abs(rs2.stddev() - rsc1.stddev_y()) < 1e-10);
            DLIB_TEST(std::abs(rs2.variance() - rsc1.variance_x()) < 1e-10);
            DLIB_TEST(std::abs(rs2.variance() - rsc1.variance_y()) < 1e-10);
            DLIB_TEST(rs2.current_n() == rsc1.current_n());

        }

        void perform_test (
        )
        {
            test_random_subset_selector();
            test_random_subset_selector2();
            test_running_covariance();
            test_running_stats();
        }
    } a;

}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with distributed alternating least squares.
 * We first create factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 =10;// 480189;//10000;
	mf_size_type size2 =10;// 17770;//10000;
	mf_size_type nnz = 10;//1408395;//1000000;
	double sigma = 1; // standard deviation
	double lambda =0;// 1/sigma/sigma;
	mf_size_type r = 5;

	// parameters for ALS
	unsigned epochs = 2;
	AlsRegularizer regularizer = ALS_L2;
	typedef SumLoss<NzslLoss, L2Loss> Loss;
	typedef NzslLoss TestLoss;
	Loss loss((NzslLoss()), L2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = 100;//nnz/100;
	BalanceType type = BALANCE_NONE;// BALANCE_L2;;
	BalanceMethod method = BALANCE_SIMPLE;

	// parameters for distribution
	int tasksPerRank = 2;
	mf_size_type blocks = world.size() * tasksPerRank;

	mfStart();

	if (world.rank() == 0) {
	#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
	#endif
		// generate original factors by sampling from a normal(0,sigma) distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
		generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and add small Gaussian noise
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		addRandom(v, random, boost::normal_distribution<>(0, 0.1));
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		v.sort();
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
		SparseMatrixCM vc;
		copyCm(v, vc);

		// create a test matrix (without noise)
		SparseMatrix vTest;
		generateRandom(vTest, testNnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Test matrix: "
			<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
		generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

		// distribute the input matrices and test matrix
		DistributedSparseMatrix dv = distributeMatrix("V", blocks, 1, true, v);
		LOG4CXX_INFO(logger, "Distributed data matrix: "
				<< dv.blocks1() << " x " << dv.blocks2() << " blocks");
		DistributedSparseMatrixCM dvc = distributeMatrix("VC", 1, blocks, false, vc);
		LOG4CXX_INFO(logger, "Distributed data matrix (CM): "
				<< dvc.blocks1() << " x " << dvc.blocks2() << " blocks");
		DistributedSparseMatrix dvTest = distributeMatrix("Vtest", blocks, blocks, true, vTest);
		LOG4CXX_INFO(logger, "Distributed test matrix: "
				<< dvTest.blocks1() << " x " << dvTest.blocks2() << " blocks");
		DistributedDenseMatrix dw = distributeMatrix("W", blocks, 1, true, w);
		DistributedDenseMatrixCM dh = distributeMatrix("H", 1, blocks, false, h);
		LOG4CXX_INFO(logger, "Distributed factor matrices");

		// initialize
		DapFactorizationData<> data(dv, dw, dh, tasksPerRank, &dvc);
		DsgdFactorizationData<> testJob(dvTest, dw, dh, tasksPerRank);
		Trace trace;
		// here add fields to Trace
//		trace.addField("balancing-type", type);
//		trace.addField("balancing-method", method);
		Timer t;

		// run ALS to try to reconstruct the original factors
		t.start();
		dalsNzsl(data, epochs, trace, lambda, regularizer, type, method, &testJob);
		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// write trace to an R file
		string typeString, methodString;
		switch (type) {
		case BALANCE_NONE:
			typeString = "None";
			break;
		case BALANCE_L2:
			typeString = "L2";
			break;
		case BALANCE_NZL2:
			typeString = "Nzl2";
			break;
		}
		switch (method) {
		case BALANCE_SIMPLE:
			methodString = "Simple";
			break;
		case BALANCE_OPTIMAL:
			methodString = "Optimal";
			break;
		}
		string filename = "/tmp/dals-trace.R";
		LOG4CXX_INFO(logger, "Writing trace to " << filename);
		trace.toRfile(filename, "dals");
	}

	mfStop();
	mfFinalize();

	return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "quad_planarity.h"
#include <Eigen/Geometry>

template <typename DerivedV, typename DerivedF, typename DerivedP>
IGL_INLINE void igl::quad_planarity(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedP> & P)
{
  int nf = F.rows();
  P.setZero(nf,1);
  for (int i =0; i<nf; ++i)
  {
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v1 = V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v2 = V.row(F(i,1));
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v3 = V.row(F(i,2));
    const Eigen::Matrix<typename DerivedV::Scalar,1,3> &v4 = V.row(F(i,3));
    Eigen::Matrix<typename DerivedV::Scalar,1,3> diagCross=(v3-v1).cross(v4-v2);
    typename Eigen::PlainObjectBase<DerivedV>::Scalar denom = diagCross.norm()*(((v3-v1).norm()+(v4-v2).norm())/2);
    if (fabs(denom)<1e-8)
      //degenerate quad is still planar
      P[i] = 0;
    else
      P[i] = (diagCross.dot(v2-v1)/denom);
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::quad_planarity<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "../include/layer.h"

int main()
{
    using namespace Eigen;
    using std::cout;
    using std::endl;
    using namespace MyDL;

    int n = 1;
    int c = 1;
    int h = 3;
    int w = 3;
    int Fn = 2;
    int Fh = 2;
    int Fw = 2;
    int stride = 1;
    int pad = 1;

    Conv2D conv(c, h, w, Fn, Fh, Fw, stride, pad, 0.1);

    vector<MatrixXd> inputs, conv_outs, pooling_outs;
    MatrixXd X = MatrixXd::Zero(n, c * h * w);
    X << 1, 2, 3, 4, 5, 6, 7, 8, 9;

    n = 1;
    c = Fn;
    h = (pad*2+h-Fh)/stride + 1;
    w = (pad*2+w-Fw)/stride + 1;
    int Ph = 2;
    int Pw = 2;
    stride = 2;
    pad = 0;

    int Oh = (2*pad + h - Ph) / stride + 1;
    int Ow = (2*pad + w - Pw) / stride + 1;

    Pooling pooling(c, h, w, Ph, Pw, stride, pad);

    // MatrixXd X = MatrixXd::Zero(n, h*w*c);

    // X << 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
    // X << 1,2,3,4,5,1,7,1,9,10,11,12,13,1,15,1;

    // vector<MatrixXd> inputs, outputs;
    // inputs.push_back(X);

    // outputs = pooling.forward(inputs);

    // cout << outputs[0] << endl;

    // vector<MatrixXd> douts, grads;
    // MatrixXd dout = MatrixXd::Zero(n, c*Oh*Ow);

    // dout << 1,2,3,4;

    // douts.push_back(dout);

    // grads = pooling.backward(douts);

    // cout << grads[0] << endl;

    // -----------------------
    //   Conv2Dとの結合の動作確認
    // -----------------------

    inputs.push_back(X);
    conv_outs = conv.forward(inputs);

    cout << conv_outs[0] << endl;

    pooling_outs = pooling.forward(conv_outs);

    cout << pooling_outs[0] << endl;

    cout << pooling._argmax << endl;

    vector<MatrixXd> douts, pooling_grads, conv_grads;
    MatrixXd dout = MatrixXd::Ones(n, Oh*Ow*c);
    dout << 1,2,3,4,1,2,3,4;
    douts.push_back(dout);

    pooling_grads = pooling.backward(douts);

    cout << pooling_grads[0] << endl;

    conv_grads = conv.backward(pooling_grads);

    cout << conv_grads[0] << endl;

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013-2015 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014, 2015, 2016, 2018.
// Modifications copyright (c) 2014-2018 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include "test_within.hpp"

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

template <typename P1, typename P2>
void test_point_box()
{
    typedef bg::model::box<P1> box_type1;
    typedef bg::model::box<P2> box_type2;

    test_geometry<P1, box_type2>("POINT(1 1)", "BOX(0 0,2 2)", true);
    test_geometry<P1, box_type2>("POINT(0 0)", "BOX(0 0,2 2)", false);
    test_geometry<P1, box_type2>("POINT(2 2)", "BOX(0 0,2 2)", false);
    test_geometry<P1, box_type2>("POINT(0 1)", "BOX(0 0,2 2)", false);
    test_geometry<P1, box_type2>("POINT(1 0)", "BOX(0 0,2 2)", false);

    test_geometry<P1, box_type2>("POINT(3 3)", "BOX(1 1,4 4)", true);
    test_geometry<P2, box_type1>("POINT(3 3)", "BOX(0 0,5 5)", true);

    test_geometry<box_type1, box_type2>("BOX(1 1,2 2)", "BOX(0 0,3 3)", true);
    test_geometry<box_type1, box_type2>("BOX(0 0,3 3)", "BOX(1 1,2 2)", false);

    test_geometry<box_type1, box_type2>("BOX(1 1,3 3)", "BOX(0 0,3 3)", true);
    test_geometry<box_type1, box_type2>("BOX(3 1,3 3)", "BOX(0 0,3 3)", false);

    test_geometry<box_type1, box_type2>("BOX(1 1,4 4)", "BOX(0 0,5 5)", true);
    test_geometry<box_type2, box_type1>("BOX(0 0,5 5)", "BOX(1 1,4 4)", false);

    /*
    test_within_code<P, box_type>("POINT(1 1)", "BOX(0 0,2 2)", 1);
    test_within_code<P, box_type>("POINT(1 0)", "BOX(0 0,2 2)", 0);
    test_within_code<P, box_type>("POINT(0 1)", "BOX(0 0,2 2)", 0);
    test_within_code<P, box_type>("POINT(0 3)", "BOX(0 0,2 2)", -1);
    test_within_code<P, box_type>("POINT(3 3)", "BOX(0 0,2 2)", -1);

    test_within_code<box_type, box_type>("BOX(1 1,2 2)", "BOX(0 0,3 3)", 1);
    test_within_code<box_type, box_type>("BOX(0 1,2 2)", "BOX(0 0,3 3)", 0);
    test_within_code<box_type, box_type>("BOX(1 0,2 2)", "BOX(0 0,3 3)", 0);
    test_within_code<box_type, box_type>("BOX(1 1,2 3)", "BOX(0 0,3 3)", 0);
    test_within_code<box_type, box_type>("BOX(1 1,3 2)", "BOX(0 0,3 3)", 0);
    test_within_code<box_type, box_type>("BOX(1 1,3 4)", "BOX(0 0,3 3)", -1);
    */
}

void test_point_box_3d()
{
    typedef boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian> point_type;
    typedef boost::geometry::model::box<point_type> box_type;
    box_type box(point_type(0, 0, 0), point_type(4, 4, 4));
    BOOST_CHECK_EQUAL(bg::within(point_type(2, 2, 2), box), true);
    BOOST_CHECK_EQUAL(bg::within(point_type(2, 4, 2), box), false);
    BOOST_CHECK_EQUAL(bg::within(point_type(2, 2, 4), box), false);
    BOOST_CHECK_EQUAL(bg::within(point_type(2, 2, 5), box), false);

    box_type box2(point_type(2, 2, 2), point_type(3, 3, 3));
    BOOST_CHECK_EQUAL(bg::within(box2, box), true);

}

template <typename P1, typename P2>
void test_point_poly()
{
    typedef boost::geometry::model::polygon<P1> poly1;
    typedef boost::geometry::model::polygon<P2> poly2;

    test_geometry<P1, poly2>("POINT(3 3)", "POLYGON((0 0,0 5,5 5,5 0,0 0))", true);
    test_geometry<P2, poly1>("POINT(3 3)", "POLYGON((0 0,0 5,5 5,5 0,0 0))", true);
}

template <typename P1, typename P2>
void test_all()
{
    test_point_box<P1, P2>();
    test_point_poly<P1, P2>();
}

template <typename P>
void test_all()
{
    test_all<P, P>();
}

void test_strategy()
{
    // Test by explicitly specifying a strategy
    typedef bg::model::d2::point_xy<double> point_type;
    typedef bg::model::box<point_type> box_type;
    point_type p(3, 3);
    box_type b(point_type(0, 0), point_type(5, 5));
    box_type b0(point_type(0, 0), point_type(5, 0));

    bool r = bg::within(p, b,
        bg::strategy::within::cartesian_point_box());
    BOOST_CHECK_EQUAL(r, true);

    r = bg::within(b, b,
        bg::strategy::within::box_in_box<box_type, box_type>());
    BOOST_CHECK_EQUAL(r, true);

    r = bg::within(b0, b0,
        bg::strategy::within::box_in_box<box_type, box_type>());
    BOOST_CHECK_EQUAL(r, false);

    r = bg::within(p, b,
        bg::strategy::within::point_in_box_by_side<>());
    BOOST_CHECK_EQUAL(r, true);
}

int test_main( int , char* [] )
{
    typedef boost::geometry::model::d2::point_xy<double> xyd;
    typedef boost::geometry::model::d2::point_xy<float> xyf;
    typedef boost::geometry::model::d2::point_xy<int> xyi;
    typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> p2d;

    test_all<xyd, p2d>();
    test_all<xyf, p2d>();
    test_all<xyi, xyd>();

    test_all<xyi>();
    test_all<xyd>();

    test_point_box_3d();
    test_strategy();

    return 0;
}