#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <iostream>

#include "common.h"
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <Eigen/Dense>

using namespace Sophus;
using namespace Eigen;
using namespace std;

struct Camera
{
    Camera() {}

    Camera(double* data)
    {
        Rt = g2o::SE3Quat::exp(Eigen::Map<Eigen::Matrix<double, 6, 1>>(data));

        f = data[6];
        k1 = data[7];
        k2 = data[8];
    }

    void set_to(double* data) const
    {
        Eigen::Matrix<double, 6, 1> rt = Rt.log();

        for (int i = 0; i < 6; ++i)
            data[i] = rt[i];

        data[6] = f;
        data[7] = k1;
        data[8] = k2;
    }

    g2o::SE3Quat Rt; // g2o::SE3Quat is stored as rotation first and then translation
    double f = 0.0, k1 = 0.0, k2 = 0.0;
};

class VertexCamera: public g2o::BaseVertex<6, Camera>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {

        _estimate = Camera();
        // _estimate = g2o::SE3Quat();
    }

    virtual void oplusImpl(const double *update) override {
        // _estimate = g2o::SE3Quat::exp(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(update)) * _estimate;
        _estimate.Rt = g2o::SE3Quat::exp(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(update)) * _estimate.Rt;

    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class VertexLandmark: public g2o::BaseVertex<3, Eigen::Vector3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Vector3d::Zero();
    }

    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Map<const Eigen::Vector3d>(update);
    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
};

class EdgeReprojection: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexLandmark, VertexCamera>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeReprojection(double f, double k1, double k2) : f_(f), k1_(k1), k2_(k2) { }

    virtual void computeError() override {
        const VertexCamera* v_cam = static_cast<VertexCamera*>(_vertices[1]);
        const VertexLandmark* v_point = static_cast<VertexLandmark*>(_vertices[0]);
        auto cam = v_cam->estimate();
        Eigen::Vector3d X = v_point->estimate();
        Eigen::Vector3d X_cam = cam.Rt * X;
        X_cam /= X_cam.z();

        // auto p2 = X_cam.x() * X_cam.x() + X_cam.y() * X_cam.y();
        // auto r = 1.0 + p2 * (k1_ + (p2 * k2_));

        Eigen::Vector2d uv = -X_cam.head<2>() *  f_ ; // minus because of the dataset projection
        _error = _measurement - uv;
    }

    virtual void linearizeOplus() override {
        const VertexCamera *v_cam = static_cast<VertexCamera*>(_vertices[1]);
        const VertexLandmark *v_landmark = static_cast<VertexLandmark*>(_vertices[0]);
        Eigen::Vector3d X = v_landmark->estimate();
        auto cam = v_cam->estimate();
        Eigen::Vector3d Xc = cam.Rt * X;

        double x = Xc.x();
        double y = Xc.y();
        double z = Xc.z();
        double z_2 = z * z;

        Eigen::Matrix<double, 2, 3> dedXc;
        dedXc << -f_ / z, 0.0, f_ * x / z_2,   // -f_ is because of the dataset projection function (not standard)
                0.0, -f_ / z, f_ * y / z_2;
        Eigen::Matrix3d dXcdR;
        dXcdR << 0.0, -z, y,
                 z, 0.0, -x,
                 -y, x, 0.0;
        Eigen::Matrix3d dXcdt = Eigen::Matrix3d::Identity();
        _jacobianOplusXj.block<2, 3>(0, 0) = dedXc * -dXcdR; // -dXcdR <=> -Xc^
        _jacobianOplusXj.block<2, 3>(0, 3) = dedXc * dXcdt;

        _jacobianOplusXi = dedXc * cam.Rt.rotation().matrix();

         _jacobianOplusXi *= -1; // take the negative jacobian because the error is defined as (measurement - projection)
         _jacobianOplusXj *= -1;

    }

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}
    private:
        double f_, k1_, k2_;

};

int main(int argc, char **argv) {

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem dataset(argv[1]);
    dataset.Normalize();
    // dataset.Perturb(0.1, 0.5, 0.5);
    dataset.WriteToPLYFile("initial_pc.ply");

    std::cout << "\n";
    std::cout << "nb cameras: " << dataset.num_cameras() << std::endl;
    std::cout << "nb landmarks: " << dataset.num_points() << std::endl;
    std::cout << "nb observations: " << dataset.num_observations() << std::endl;
    std::cout << "nb parameters: " << dataset.num_parameters() << std::endl;
    std::cout << "check: " << dataset.num_cameras() * 9 + dataset.num_points()*3 << std::endl;

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;

    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
    );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    auto* cameras = dataset.mutable_cameras();
    std::vector<VertexCamera*> camera_vertices;
    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        auto *c = new VertexCamera();
        c->setId(i);
        c->setEstimate(Camera(cameras + (i*dataset.camera_block_size())));
        optimizer.addVertex(c);
        camera_vertices.push_back(c);
    }

    auto* landmarks = dataset.mutable_points();
    std::vector<VertexLandmark*> landmark_vertices;
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        auto* l = new VertexLandmark();
        l->setId(dataset.num_cameras() + i);
        l->setEstimate(Eigen::Map<Eigen::Vector3d>(landmarks + i*dataset.point_block_size()));
        l->setMarginalized(true);
        optimizer.addVertex(l);
        landmark_vertices.push_back(l);
    }

    auto* observations = dataset.observations();
    auto* cam_indices = dataset.camera_index();
    auto* landmark_indices = dataset.point_index();
    for (int i = 0; i < dataset.num_observations(); ++i)
    {
        auto c = Camera(cameras + (cam_indices[i]*dataset.camera_block_size()));
        auto* e = new EdgeReprojection(c.f, c.k1, c.k2);
        e->setVertex(1, camera_vertices[cam_indices[i]]);
        e->setVertex(0, landmark_vertices[landmark_indices[i]]);
        e->setMeasurement(Eigen::Map<const Eigen::Vector2d>(observations + i*2));
        e->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(e);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        camera_vertices[i]->estimate().set_to(cameras + (i * dataset.camera_block_size()));
        // auto v = camera_vertices[i]->estimate().log();
        // double* pt = cameras + (i * dataset.camera_block_size());
        // for (int j = 0; j < 6; ++j)
        // {
        //     pt[j] = v[j];
        // }
    }
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        Eigen::Vector3d X = landmark_vertices[i]->estimate();
        landmarks[i*3] = X.x();
        landmarks[i*3+1] = X.y();
        landmarks[i*3+2] = X.z();
    }

    dataset.WriteToPLYFile("after_ba_g2o.ply");

    return 0;
}


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_LINEAR_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_LINEAR_HPP

#include <random>
#include <iterator>
#include <algorithm>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>
#include <boost/geometry/extensions/random/strategies/cartesian/uniform_point_distribution_segment.hpp>
#include <boost/geometry/extensions/random/strategies/spherical/edwilliams_avform_intermediate.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

template
<
    typename Point,
    typename DomainGeometry,
    typename SegmentStrategy = services::default_strategy
        <
            typename point_type<DomainGeometry>::type,
            model::segment<typename point_type<DomainGeometry>::type>
        >
>
class uniform_linear
{
private:
    typedef typename default_length_result<DomainGeometry>::type length_type;
    typedef typename point_type<DomainGeometry>::type domain_point_type;
    std::vector<std::size_t> skip_list;
    std::vector<domain_point_type> point_cache;
    std::vector<length_type> accumulated_lengths;
public:
    uniform_linear(DomainGeometry const& g)
    {
        std::size_t i = 0;
        point_cache.push_back(*segments_begin(g)->first);
        accumulated_lengths.push_back(0);
        for (auto it = segments_begin(g) ; it != segments_end(g) ; ++it)
        {
            accumulated_lengths.push_back(
                accumulated_lengths.back() + length(*it));
            if (!boost::geometry::equals(point_cache.back(), *it->first))
            {
                point_cache.push_back(*it->first);
                skip_list.push_back(i);
            }
            point_cache.push_back(*it->second);
            ++i;
        }
    }
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_linear const& r_strategy) const
    {
        if(r_strategy.skip_list.size() != skip_list.size()
            || r_strategy.point_cache.size() != point_cache.size())
            return false;
        for (std::size_t i = 0; i < skip_list.size(); ++i)
        {
            if (skip_list[i] != r_strategy.skip_list[i]) return false;
        }
        for (std::size_t i = 0; i < point_cache.size(); ++i)
        {
            if (!boost::geometry::equals(point_cache[i], r_strategy.point_cache[i])) return false;
        }
        return true;
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        typedef typename select_most_precise
            <
                double,
                typename coordinate_type<Point>::type
            >::type sample_type;
        std::uniform_real_distribution<sample_type> dist(0, accumulated_lengths.back());
        sample_type r = dist(g);
        std::size_t i = std::distance(
            accumulated_lengths.begin(),
            std::lower_bound(accumulated_lengths.begin(),
                             accumulated_lengths.end(),
                             r));
        std::size_t offset = std::distance(
            skip_list.begin(),
            std::lower_bound(skip_list.begin(), skip_list.end(), i));

        return SegmentStrategy::template map
            <
                sample_type,
                domain_point_type
            >(point_cache[ i + offset - 1 ], point_cache[ i + offset ],
                ( r - accumulated_lengths[ i - 1 ]) /
                ( accumulated_lengths[ i ] - accumulated_lengths[ i - 1 ] ));
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry,
    typename MultiOrSingle,
    int Dim,
    typename CsTag
>
struct default_strategy
<
    Point,
    DomainGeometry,
    linear_tag,
    MultiOrSingle,
    Dim,
    CsTag
> : public uniform_linear<Point, DomainGeometry> {
    typedef uniform_linear<Point, DomainGeometry> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif //  BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_LINEAR_HPP


#include <antgame/FoodSource.h>
#include <antgame/Food.h>
#include <antgame/World.h>

#include <easyloggingpp/easylogging++.h>

#include <boost/geometry.hpp>

#include <random>

void FoodSource::Update(const WorldTree& world) {
    static std::default_random_engine generator;
    static size_t totalFoodCounter = 0;
    static constexpr auto kFoodValue = 20;
    static constexpr auto kMaxFood = 500;

    int numberOfFood = m_poissonDist(generator);
    m_numFood += numberOfFood;

    if (m_numFood > kMaxFood) {
        return;
    }

    for (size_t i = 0; i < numberOfFood; ++i) {
        auto x = m_uniformDistX(generator);
        auto y = m_uniformDistY(generator);
        Point point(x, y);
        auto newObj = std::make_shared<Food>(point, "Food_" + std::to_string(totalFoodCounter), kFoodValue, this);
        m_world->AddObject(std::move(newObj));
    }
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 */

#define BOOST_TEST_MAIN

#include <map>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/nearestNeighbourSearch.h"

namespace tudat
{
namespace unit_tests
{

//! Define Boost test suite.
BOOST_AUTO_TEST_SUITE( test_basic_functions )

//! Test if search for nearest left neighbor using binary search works correctly.
BOOST_AUTO_TEST_CASE( testNearestLeftNeighborUsingBinarySearch )
{
    using namespace basic_mathematics;

    // Case 1: test Eigen-interface.
    {
        // Populate vector of 10 sorted elements.
        Eigen::VectorXd vectorOfSortedData( 10 );
        vectorOfSortedData << 1.0, 4.5, 10.6, 14.98, 54.65, 88.9, 101.31, 144.63, 180.01, 201.94;

        // Declare vector of target values.
        Eigen::VectorXd vectorOfTargetValues( 5 );
        vectorOfTargetValues << 1.1, 4.6, 10.5, 54.55, 181.63;

        // Declare vector of expected indices.
        Eigen::VectorXi vectorOfExpectedIndices( 5 );
        vectorOfExpectedIndices << 0, 1, 1, 3, 8;

        // Compute nearest left neighbors and check if they match expectations.
        for ( int i = 0; i < vectorOfTargetValues.rows( ); i++ )
        {
            BOOST_CHECK_EQUAL(
                        vectorOfExpectedIndices[ i ],
                        computeNearestLeftNeighborUsingBinarySearch(
                            vectorOfSortedData, vectorOfTargetValues[ i ] ) );
        }
    }

    // Case 2: test map-interface with VectorXd.
    {
        // Populate map of 10 sorted elements.
        std::map< double, Eigen::VectorXd > mapOfSortedData;

        Eigen::VectorXd vectorOfData( 1 );
        vectorOfData << 1.0;

        mapOfSortedData[ 0.3 ] = vectorOfData;
        mapOfSortedData[ 3.65 ] = vectorOfData;
        mapOfSortedData[ 43.12 ] = vectorOfData;
        mapOfSortedData[ 2.23 ] = vectorOfData;
        mapOfSortedData[ 1.233 ] = vectorOfData;
        mapOfSortedData[ 6.78 ] = vectorOfData;
        mapOfSortedData[ 0.21 ] = vectorOfData;
        mapOfSortedData[ -1.23 ] = vectorOfData;
        mapOfSortedData[ -931.12 ] = vectorOfData;
        mapOfSortedData[ 124.52 ] = vectorOfData;

        // Declare vector of target values.
        Eigen::VectorXd vectorOfTargetValues( 5 );
        vectorOfTargetValues << -1.22, 3.66, -931.11, 43.12, 0.4;

        // Declare vector of expected indices.
        Eigen::VectorXi vectorOfExpectedIndices( 5 );
        vectorOfExpectedIndices << 1, 6, 0, 8, 3;

        // Compute nearest left neighbors and check if they match expectations.
        for ( int i = 0; i < vectorOfTargetValues.rows( ); i++ )
        {
            BOOST_CHECK_EQUAL(
                        vectorOfExpectedIndices[ i ],
                        computeNearestLeftNeighborUsingBinarySearch(
                            mapOfSortedData, vectorOfTargetValues[ i ] ) );
        }
    }

    // Case 3: test templated STL vector-interface.
    {
        // Populate vector of 10 sorted elements.
        std::vector< double > vectorOfSortedData( 10 );
        vectorOfSortedData[ 0 ] = 1.0;
        vectorOfSortedData[ 1 ] = 4.5;
        vectorOfSortedData[ 2 ] = 10.6;
        vectorOfSortedData[ 3 ] = 14.98;
        vectorOfSortedData[ 4 ] = 54.65;
        vectorOfSortedData[ 5 ] = 88.9;
        vectorOfSortedData[ 6 ] = 101.31;
        vectorOfSortedData[ 7 ] = 144.63;
        vectorOfSortedData[ 8 ] = 180.01;
        vectorOfSortedData[ 9 ] = 201.94;

        // Declare vector of target values.
        std::vector< double > vectorOfTargetValues( 5 );
        vectorOfTargetValues[ 0 ] = 1.1;
        vectorOfTargetValues[ 1 ] = 4.6;
        vectorOfTargetValues[ 2 ] = 10.5;
        vectorOfTargetValues[ 3 ] = 54.55;
        vectorOfTargetValues[ 4 ] = 181.63;

        // Declare vector of expected indices.
        std::vector< double > vectorOfExpectedIndices( 5 );
        vectorOfExpectedIndices[ 0 ] = 0;
        vectorOfExpectedIndices[ 1 ] = 1;
        vectorOfExpectedIndices[ 2 ] = 1;
        vectorOfExpectedIndices[ 3 ] = 3;
        vectorOfExpectedIndices[ 4 ] = 8;

        // Compute nearest left neighbors and check if they match expectations.
        for ( int i = 0; i < 5; i++ )
        {
            BOOST_CHECK_EQUAL( vectorOfExpectedIndices[ i ],
             computeNearestLeftNeighborUsingBinarySearch< double >(
                            vectorOfSortedData, vectorOfTargetValues[ i ] ) );
        }
    }

    // Case 4: test templated hunting algorithm ( with STL vector-interface ).
    {
        // Populate vector of 10 sorted elements.
        std::vector< double > vectorOfSortedData( 10 );
        vectorOfSortedData[ 0 ] = 1.0;
        vectorOfSortedData[ 1 ] = 4.5;
        vectorOfSortedData[ 2 ] = 10.6;
        vectorOfSortedData[ 3 ] = 14.98;
        vectorOfSortedData[ 4 ] = 54.65;
        vectorOfSortedData[ 5 ] = 88.9;
        vectorOfSortedData[ 6 ] = 101.31;
        vectorOfSortedData[ 7 ] = 144.63;
        vectorOfSortedData[ 8 ] = 180.01;
        vectorOfSortedData[ 9 ] = 201.94;

        // Declare vector of target values.
        std::vector< double > vectorOfTargetValues( 5 );
        vectorOfTargetValues[ 0 ] = 1.1;
        vectorOfTargetValues[ 1 ] = 4.6;
        vectorOfTargetValues[ 2 ] = 10.5;
        vectorOfTargetValues[ 3 ] = 54.55;
        vectorOfTargetValues[ 4 ] = 181.63;

        // Declare vector of expected indices.
        std::vector< double > vectorOfExpectedIndices( 5 );
        vectorOfExpectedIndices[ 0 ] = 0;
        vectorOfExpectedIndices[ 1 ] = 1;
        vectorOfExpectedIndices[ 2 ] = 1;
        vectorOfExpectedIndices[ 3 ] = 3;
        vectorOfExpectedIndices[ 4 ] = 8;

        // Compute nearest left neighbors and check if they match expectations.
        for ( int i = 0; i < 5; i++ )
        {
            // Check whether each initial guess yields correct result.
            for( int j = 0; j < 9; j++ )
            {
                BOOST_CHECK_EQUAL( vectorOfExpectedIndices[ i ],
                    findNearestLeftNeighbourUsingHuntingAlgorithm< double >(
                    vectorOfTargetValues[ i ], j, vectorOfSortedData ) );
            }
        }
    }

    // Case 5: test Eigen-interface of NearestNeighbourSearch.
    {
        // Populate vector of 10 sorted elements.
        Eigen::VectorXd vectorOfSortedData( 10 );
        vectorOfSortedData << 1.0, 4.5, 10.6, 14.98, 54.65, 88.9, 101.31, 144.63, 180.01, 201.94;

        // Declare vector of target values.
        Eigen::VectorXd vectorOfTargetValues( 10 );
        vectorOfTargetValues << 1.1, 2.74, 2.75, 2.76, 4.6, 10.5, 54.55, 181.63, 200.0, 205.0;

        // Declare vector of expected indices.
        Eigen::VectorXi vectorOfExpectedIndices( 10 );
        vectorOfExpectedIndices << 0, 0, 0, 1, 1, 2, 4, 8, 9, 9;

        // Compute nearest left neighbors and check if they match expectations.
        for ( int i = 0; i < vectorOfTargetValues.rows( ); i++ )
        {
            BOOST_CHECK_EQUAL(
                        vectorOfExpectedIndices[ i ],
                        computeNearestNeighborUsingBinarySearch(
                            vectorOfSortedData, vectorOfTargetValues[ i ] ) );
        }
    }

}

//! Close Boost test suite.
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/fnma.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

STF_CASE_TPL(" fnma",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::fnma;

  STF_EXPR_IS(fnma(T(),T(),T()), T);

  STF_EQUAL(fnma(T(4),T(2),T(2)) , T(-10));
  STF_EQUAL(fnma(T(4),T(-2),T(2)), T(6));
  STF_EQUAL(fnma(T(4),T(2),T(-2)), T(-6));
  STF_EQUAL(fnma(T(4),T(-2),T(-2)), T(10));
}


#pragma once

#include <polyfem/Mesh2D.hpp>
#include <polyfem/ElementBases.hpp>
#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/InterfaceData.hpp>
#include <polyfem/LocalBoundary.hpp>

#include <Eigen/Dense>
#include <vector>
#include <map>

namespace polyfem
{

	class MVPolygonalBasis2d
	{
	public:
		static int build_bases(
			const std::string &assembler_name,
			const Mesh2D &mesh,
			const int n_bases,
			const int quadrature_order,
			std::vector< ElementBases > &bases,
			const std::vector< ElementBases > &gbases,
			const  std::map<int, InterfaceData> &poly_edge_to_data,
			std::vector<LocalBoundary> &local_boundary,
			std::map<int, Eigen::MatrixXd> &mapped_boundary);

		static void meanvalue(const Eigen::MatrixXd &polygon, const Eigen::RowVector2d &point, Eigen::MatrixXd &b, const double tol);
		static void meanvalue_derivative(const Eigen::MatrixXd &polygon, const Eigen::RowVector2d &point, Eigen::MatrixXd &derivatives, const double tol);
	};
}


#ifndef BLUB_PROCEDURAL_VOXEL_EDIT_NOISE_HPP
#define BLUB_PROCEDURAL_VOXEL_EDIT_NOISE_HPP

#include "blub/core/vector.hpp"
#include "blub/math/axisAlignedBox.hpp"
#include "blub/procedural/log/global.hpp"
#include "blub/procedural/voxel/edit/base.hpp"

#include <boost/function/function2.hpp>

#include <numeric>

namespace blub
{
namespace procedural
{
namespace voxel
{
namespace edit
{

/**
 * @brief The noise class generates a random terrain using simplex noise http://en.wikipedia.org/wiki/Simplex_noise .
 * Original code http://webstaff.itn.liu.se/~stegu/simplexnoise/SimplexNoise.java
 */
template <class configType>
class noise : public base<configType>
{
public:
    typedef boost::function<bool (vector3, real&)> t_callbackInterpolation;

    typedef configType t_config;
    typedef base<t_config> t_base;
    typedef sharedPointer<noise> pointer;
    typedef typename t_config::t_data t_voxel;

    /**
     * @brief creates an instance of the class and returns it as shared_ptr<>
     * @param desc Defines the size in which the terrain should get generated.
     * @param scale Gets used to scale the voxel-position before calculating the interpolation
     * @param seed Used before calling std::random_shuffle
     * @param callbackInterpolation Callback for the generated interpolation.
     * @return An instance of the class as shared_ptr<>
     */
    static pointer create(const blub::axisAlignedBox& desc,
                          const vector3& scale,
                          const uint32& seed = 0,
                          const t_callbackInterpolation& callbackInterpolation = [] (const vector3&, real& value) {value*=1024;return true;})
    {
        return pointer(new noise(desc, scale, seed, callbackInterpolation));
    }
    /**
     * @brief ~noise destructor
     */
    virtual ~noise()
    {
        ;
    }

    /**
     * @brief getAxisAlignedBoundingBox returns transformed aab of the to generate voxel
     * @param trans Transform
     * @return
     */
    blub::axisAlignedBox getAxisAlignedBoundingBox(const transform& trans) const override
    {
        return blub::axisAlignedBox(m_aab.getMinimum() + trans.position,
                                    m_aab.getMaximum() + trans.position);
    }

protected:
    static real fade(const real &t)
    {
        return t * t * t * (t * (t * 6 - 15) + 10);
    }
    static real lerp(const real& t, const real& a, const real& b)
    {
        return a + t * (b - a);
    }
    static real grad(const int32& hash, const real& x, const real& y, const real& z) {
        const int32 h = hash & 15;                      // CONVERT LO 4 BITS OF HASH CODE
        const real u = h < 8 ? x : y,                   // INTO 12 GRADIENT DIRECTIONS.
                   v = h < 4 ? y : h == 12 || h == 14 ? x : z;
        return ((h & 1) == 0 ? u : -u) + ((h & 2) == 0 ? v : -v);
    }
    int32 permutation(const int32 index) const
    {
        BASSERT(index >= 0);
        BASSERT(index < 512);
        return m_permutation[index%256];
    }

    /**
     * @brief calculateOneVoxel scales pos by scale set in constructor and calculates the noise.
     * @param pos absolute voxel-position
     * @param resultVoxel gets set if interpolation larger -127
     * @return Returns true if interpolation larger -127
     */
    bool calculateOneVoxel(const vector3& pos, t_voxel* resultVoxel) const override
    {
        real x(pos.x*m_scale.x);
        real y(pos.y*m_scale.y);
        real z(pos.z*m_scale.z);

        const int32 X = static_cast<int32>(math::floor(x)) & 255;                  // FIND UNIT CUBE THAT
        const int32 Y = static_cast<int32>(math::floor(y)) & 255;                  // CONTAINS POINT.
        const int32 Z = static_cast<int32>(math::floor(z)) & 255;
        x -= static_cast<int32>(math::floor(x));                                // FIND RELATIVE X,Y,Z
        y -= static_cast<int32>(math::floor(y));                                // OF POINT IN CUBE.
        z -= static_cast<int32>(math::floor(z));
        const real u = fade(x);                                // COMPUTE FADE CURVES
        const real v = fade(y);                                // FOR EACH OF X,Y,Z.
        const real w = fade(z);
        const int32 A =  permutation(X  )+Y;
        const int32 AA = permutation(A)+Z;
        const int32 AB = permutation(A+1)+Z;      // HASH COORDINATES OF
        const int32 B =  permutation(X+1)+Y;
        const int32 BA = permutation(B)+Z;
        const int32 BB = permutation(B+1)+Z;      // THE 8 CUBE CORNERS,

        real resultInterpolation =   lerp(w, lerp(v, lerp(u, grad(permutation(AA  ), x  , y  , z   ),  // AND ADD
                                                             grad(permutation(BA  ), x-1, y  , z   )), // BLENDED
                                                     lerp(u, grad(permutation(AB  ), x  , y-1, z   ),  // RESULTS
                                                             grad(permutation(BB  ), x-1, y-1, z   ))),// FROM  8
                                             lerp(v, lerp(u, grad(permutation(AA+1), x  , y  , z-1 ),  // CORNERS
                                                             grad(permutation(BA+1), x-1, y  , z-1 )), // OF CUBE
                                                     lerp(u, grad(permutation(AB+1), x  , y-1, z-1 ),
                                                             grad(permutation(BB+1), x-1, y-1, z-1 ))));
        if (!m_callbackInterpolation(pos, resultInterpolation))
        {
            return false;
        }
        const int8 resultCasted(static_cast<int8>(math::clamp<real>(resultInterpolation, -127., 127.)));

        resultVoxel->setInterpolation(resultCasted);

        return true;
    }

    /**
     * @brief noise Contructor - same as create()
     */
    noise(const blub::axisAlignedBox& desc, const vector3& scale, const uint32& seed, const t_callbackInterpolation &callbackInterpolation)
        : m_aab(desc)
        , m_scale(scale)
        , m_callbackInterpolation(callbackInterpolation)
        , m_permutation(256)
    {
        std::srand(seed);
        std::iota(m_permutation.begin(), m_permutation.end(), 0);
        std::random_shuffle(m_permutation.begin(), m_permutation.end());
    }

protected:
    const blub::axisAlignedBox m_aab;
    const vector3 m_scale;
    const t_callbackInterpolation m_callbackInterpolation;

    vector<uint8_t> m_permutation;

};

}
}
}
}

#endif // BLUB_PROCEDURAL_VOXEL_EDIT_NOISE_HPP


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticMarginalAlphaFunction.hpp
//! \author Alex Robinson
//! \brief  Free gas elastic marginal alpha function definition.
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "DataGen_FreeGasElasticMarginalAlphaFunction.hpp"
#include "Utility_SearchAlgorithms.hpp"
#include "MonteCarlo_KinematicHelpers.hpp"
#include "Utility_DesignByContract.hpp"

namespace DataGen{

// Constructor
FreeGasElasticMarginalAlphaFunction::FreeGasElasticMarginalAlphaFunction(
          const std::shared_ptr<Utility::UnivariateDistribution>&
          zero_temp_elastic_cross_section,
          const std::shared_ptr<MonteCarlo::NuclearScatteringAngularDistribution>&
          cm_scattering_distribution,
          const double A,
          const double kT,
          const double beta,
          const double E )
  : d_gkq_set( 1e-4 ),
    d_sab_function( zero_temp_elastic_cross_section,
                    cm_scattering_distribution,
                    A,
                    kT ),
    d_beta( beta ),
    d_E( E ),
    d_alpha_min( 0.0 ),
    d_alpha_max( 0.0 ),
    d_norm_constant( 1.0 ),
    d_cached_cdf_values()
{
  // Make sure the values are valid
  testPrecondition( A > 0.0 );
  testPrecondition( kT > 0.0 );
  testPrecondition( E > 0.0 );
  testPrecondition( beta > MonteCarlo::calculateBetaMin( E, kT ) );

  updateCachedValues();
}

// Set the beta and energy values
void FreeGasElasticMarginalAlphaFunction::setIndependentVariables(
                                                             const double beta,
                                                             const double E )
{
  // Make sure beta is valid
  remember( double kT = d_sab_function.getTemperature() );
  testPrecondition( beta > MonteCarlo::calculateBetaMin( E, kT ) );

  d_beta = beta;
  d_E = E;

  updateCachedValues();
}

// Get the lower alpha limit
double FreeGasElasticMarginalAlphaFunction::getAlphaMin() const
{
  return d_alpha_min;
}

// Get the upper alpha limit
double FreeGasElasticMarginalAlphaFunction::getAlphaMax() const
{
  return d_alpha_max;
}

// Get the normalization constant
double FreeGasElasticMarginalAlphaFunction::getNormalizationConstant() const
{
  return d_norm_constant;
}

// Evaluate the marginal PDF
double FreeGasElasticMarginalAlphaFunction::operator()( const double alpha )
{
  // Make sure the alpha value is valid
  testPrecondition( alpha >= d_alpha_min );
  testPrecondition( alpha <= d_alpha_max );

  return d_sab_function( alpha, d_beta, d_E )/d_norm_constant;
}

// Evaluate the marginal CDF
double FreeGasElasticMarginalAlphaFunction::evaluateCDF( const double alpha )
{
  // Make sure the alpha value is valid
  testPrecondition( alpha >= d_alpha_min );
  testPrecondition( alpha <= d_alpha_max );

  // Find nearest cached evaluation of cdf
  std::list<std::pair<double,double> >::iterator lower_cdf_point =
    d_cached_cdf_values.begin();

  lower_cdf_point = Utility::Search::binaryLowerBound<Utility::FIRST>(
                                                     lower_cdf_point,
                                                     d_cached_cdf_values.end(),
                                                     alpha );

  // Calculate the cdf value
  double cdf_value, cdf_value_error;

  d_gkq_set.integrateAdaptively<15>( *this,
                                    lower_cdf_point->first,
                                    alpha,
                                    cdf_value,
                                    cdf_value_error );

  cdf_value += lower_cdf_point->second;

  // Cache the new cdf value
  std::pair<double,double> new_cdf_point(alpha, cdf_value );

  d_cached_cdf_values.insert( ++lower_cdf_point, new_cdf_point );

  // Return the calculated cdf value
  return cdf_value;
}

// Update cached values
void FreeGasElasticMarginalAlphaFunction::updateCachedValues()
{
  // Calculate the alpha limits
  double A = d_sab_function.getAtomicWeightRatio();
  double kT = d_sab_function.getTemperature();

  d_alpha_min = MonteCarlo::calculateAlphaMin( d_E, d_beta, A, kT );

  d_alpha_max = MonteCarlo::calculateAlphaMax( d_E, d_beta, A, kT );

  // Calculate the norm constant
  double norm_constant_error;

  boost::function<double (double alpha)> sab_function_wrapper =
    boost::bind<double>( d_sab_function, _1, d_beta, d_E );

  d_gkq_set.integrateAdaptively<15>( sab_function_wrapper,
                                    d_alpha_min,
                                    d_alpha_max,
                                    d_norm_constant,
                                    norm_constant_error );

  // Reset the cached cdf values
  d_cached_cdf_values.clear();

  // Add cdf point at alpha min
  d_cached_cdf_values.push_back( std::make_pair(d_alpha_min, 0.0) );

  // Add cdf point at alpha max
  d_cached_cdf_values.push_back( std::make_pair(d_alpha_max, 1.0) );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticMarginalAlphaFunction.cpp
//---------------------------------------------------------------------------//


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/QR>
#include <manifold/SO3.h>

std::vector<Eigen::Matrix3f> Gs_;
float tau_R_;
Eigen::Matrix3f Sigma_t_;

struct Stats {
  uint32_t N;
  Eigen::Vector3f sum;
  Eigen::Matrix3f outer;
  void reset(double regularize) {
    N = 0;
    sum.fill(0.);
    outer = Eigen::Matrix3f::Identity()*regularize;
  }
  void add(const Eigen::Vector3f& x) {
    ++N;
    sum += x;
    outer += x*x.transpose();
  }
};

Eigen::Matrix<float,6,1> OdomJacobian(
  const Eigen::Vector3f& t, const Eigen::Vector3f& t_prev,
  const Eigen::Matrix3f& R, const Eigen::Matrix3f& R_prev) {

  Eigen::Matrix<float,6,1> J;
  J.topRows<3>() = Sigma_t_.ldlt().solve(t-t_prev);
  for (uint32_t l=0; l<3; ++l)
    J(l+3) = -tau_R_*(R_prev.transpose()*Gs_[l]*R).trace();
  return J;
}

Eigen::Matrix<float,6,6> OdomHessian(
  const Eigen::Vector3f& t, const Eigen::Vector3f& t_prev,
  const Eigen::Matrix3f& R, const Eigen::Matrix3f& R_prev) {
  Eigen::Matrix<float,6,6> H = Eigen::Matrix<float,6,6>::Zero();
  H.topLeftCorner<3,3>() = Sigma_t_.inverse();
  for (uint32_t l=0; l<3; ++l) for (uint32_t m=0; m<3; ++m) {
    Eigen::Matrix3f Glmml = (Gs_[l]*Gs_[m] + Gs_[m]*Gs_[l]);
    H(l+3,m+3) = -0.5*tau_R_*(R_prev.transpose()*Glmml*R).trace();
  }
  return H;
};

void AddObsHessian(
  const Eigen::Vector3f& x,
  const Eigen::Vector3f& y,
  const Eigen::Matrix3f& covInv,
  double w,
  Eigen::Matrix<float,6,6>& H) {

  H.topLeftCorner<3,3>() += w*covInv;

  for (uint32_t m=0; m<3; ++m) {
    H.block<3,1>(0,m+3) += w*covInv*Gs_[m]*y;
  }

  for (uint32_t l=0; l<3; ++l) for (uint32_t m=0; m<3; ++m) {
    Eigen::Matrix3f Glmml = (Gs_[l]*Gs_[m] + Gs_[m]*Gs_[l]);
    H(l+3,m+3) += w*(-y.dot(Gs_[l]*covInv*Gs_[m]*y)
//        + 2.*y.dot(covInv*Glmml*y)
        +0.5*x.dot(covInv*Glmml*y));
  }
  H.bottomLeftCorner<3,3>() = H.topRightCorner<3,3>().transpose();
}

void AddClusterHessian(
    const Stats& ss,
    const Eigen::Matrix3f& R,
    const Eigen::Matrix3f& covInv,
    const Eigen::LDLT<Eigen::Matrix3f>& covLdlt,
    const Eigen::Vector3f& a,
    Eigen::Matrix<float,6,6>& H) {

  H.topLeftCorner<3,3>() += ss.N*covInv;
  for (uint32_t m=0; m<3; ++m) {
    H.block<3,1>(0,m+3) += covLdlt.solve(Gs_[m]*R*ss.sum);
  }
  Eigen::Matrix3f Hw;
  for (uint32_t l=0; l<3; ++l) for (uint32_t m=0; m<3; ++m) {
    Eigen::Matrix3f Glmml = (Gs_[l]*Gs_[m]+Gs_[m]*Gs_[l]);
//    H(l+3,m+3) +=
    Hw(l,m) = 0.5*(R*ss.outer*R.transpose()*(covLdlt.solve(Glmml)
          -2.*Gs_[l]*covLdlt.solve(Gs_[m]))).trace()
      +0.5*a.dot(covLdlt.solve(Glmml*R*ss.sum));
  }
//  std::cout << Hw << std::endl;
  H.bottomRightCorner<3,3>() += Hw;
  H.bottomLeftCorner<3,3>() = H.topRightCorner<3,3>().transpose();
}

void AddObsJacobian( const Eigen::Vector3f& x, const Eigen::Vector3f&
    y, const Eigen::Matrix3f& covInv, float w,
    Eigen::Matrix<float,6,1>& J) {
  J.topRows<3>() += w*covInv*x;
  for (uint32_t l=0; l<3; ++l)
    J(l+3) += w*x.dot(covInv*Gs_[l]*y);
}
void AddClusterJacobian(
    const Stats& ss,
    const Eigen::Matrix3f& R,
    const Eigen::LDLT<Eigen::Matrix3f>& covLdlt,
    const Eigen::Vector3f& a,
    Eigen::Matrix<float,6,1>& J) {

  J.topRows<3>() += covLdlt.solve(R*ss.sum + ss.N*a);
  for (uint32_t l=0; l<3; ++l)
    J(l+3) += (R*ss.outer*R.transpose()*covLdlt.solve(Gs_[l])).trace()
      + a.dot(covLdlt.solve(Gs_[l]*R*ss.sum));
}

int main (int argc, char** argv) {

  Gs_ = std::vector<Eigen::Matrix3f>(3,Eigen::Matrix3f::Zero());
  // so(3) generators
  Gs_[0](1,2) = -1;
  Gs_[0](2,1) = 1;
  Gs_[1](0,2) = 1;
  Gs_[1](2,0) = -1;
  Gs_[2](0,1) = -1;
  Gs_[2](1,0) = 1;
  // Basically ignore priors
  tau_R_ = 0.00001;
  Sigma_t_ = 10000.*Eigen::Matrix3f::Identity();

  uint32_t K = 4;
  uint32_t N = 40;

  double theta0 = 0.*M_PI/180.;
  Eigen::Matrix3f Rmu;
  Rmu << 1, 0, 0,
         0, cos(theta0), sin(theta0),
         0, -sin(theta0), cos(theta0);
  Eigen::Matrix3f R = Rmu;
  Eigen::Matrix3f covInv = 0.01*Eigen::Matrix3f::Identity();

  Eigen::Vector3f t(0,0,0.);
  Eigen::Vector3f t_prev(0,0,0);

  std::vector<Eigen::Vector3f> mus;
  mus.push_back(Eigen::Vector3f(1.,0.,0.));
  mus.push_back(Eigen::Vector3f(0.,1.,0.));
  mus.push_back(Eigen::Vector3f(0.,0.,1.));
  mus.push_back(Eigen::Vector3f(0.3,0.3,0.3));

  double theta = 15.*M_PI/180.;
  double phi = 25*M_PI/180.;
  std::vector<Eigen::Vector3f> ps;
  std::vector<uint32_t> zs;
  Eigen::Vector3f t_true = Eigen::Vector3f(0.1,.1,0.1);
  Eigen::Matrix3f R_true;
  R_true << 1, 0, 0,
            0,  cos(theta), sin(theta),
            0, -sin(theta), cos(theta);
  Eigen::Matrix3f R2;
  R2     << cos(phi),  0,  sin(phi),
            0,           1., 0,
            -sin(phi), 0,  cos(phi);
  R_true *= R2;
//  R_true << sin(theta)*cos(phi), cos(theta)*cos(phi), -sin(phi),
//            sin(theta)*sin(phi), cos(theta)*sin(phi), cos(phi),
//            cos(theta), -sin(theta), 0.;

  for (uint32_t i=0; i<N/K; ++i) {
    ps.push_back(Eigen::Vector3f(1.,0,0));
    ps.back() = R_true*ps.back() + t_true;
    ps.push_back(Eigen::Vector3f(0,1.,0.));
    ps.back() = R_true*ps.back() + t_true;
    ps.push_back(Eigen::Vector3f(0,0.,1.));
    ps.back() = R_true*ps.back() + t_true;
    ps.push_back(Eigen::Vector3f(0.3,0.3,0.3));
    ps.back() = R_true*ps.back() + t_true;
    zs.push_back(0);
    zs.push_back(1);
    zs.push_back(2);
    zs.push_back(3);
  }

  std::vector<Stats> ss(K);
  for (uint32_t k=0; k<K; ++k) ss[k].reset(0.);
  for (uint32_t i=0; i<N; ++i) {
    ss[zs[i]].add(ps[i]);
  }
  for (uint32_t k=0; k<K; ++k) {
    std::cout << k <<  std::endl;
    std::cout << ss[k].N << std::endl;
    std::cout << ss[k].sum.transpose() << std::endl;
    std::cout << ss[k].outer << std::endl;
  }
  Eigen::LDLT<Eigen::Matrix3f> covLdlt(covInv.inverse());

  std::cout << "Using SO(3) formulation first order -------------------"
    << std::endl;

  R = Rmu;
  t.fill(0);
  float delta = 1.;
  float f_prev = 1e99;
  float f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f y = R*ps[i];
      const Eigen::Vector3f x = y+t-mus[zs[i]];
      AddObsJacobian(x,y,covInv,1.,J);
    }
    J = -delta*J;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%100==0)
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }

  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << std::endl << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;
  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

  std::cout << "Using SO(3) formulation first order SS -------------------"
    << std::endl;

  R = Rmu;
  t.fill(0);
  delta = 1.;
  f_prev = 1e99;
  f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    for (uint32_t k=0; k<K; ++k) {
      const Eigen::Vector3f a = t-mus[k];
      AddClusterJacobian(ss[k],R,covLdlt,a,J);
    }
    J = -delta*J;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%100==0)
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }

  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << std::endl << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;
  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

  std::cout << "Using SO(3) formulation second order -------------------"
    << std::endl;
  R = Rmu;
  t.fill(0);
  delta = 0.5;
  f_prev = 1e99;
  f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  Eigen::Vector3f tPrev = t;
  Eigen::Matrix3f RPrev = R;
  for (uint32_t it=0; it<2000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    Eigen::Matrix<float,6,6> H = OdomHessian(t, t_prev, R, Rmu);
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f y = R*ps[i];
      const Eigen::Vector3f x = y+t-mus[zs[i]];
      AddObsJacobian(x,y,covInv,1.,J);
      AddObsHessian(x,y,covInv,1.,H);
    }
    if(it < 10) {
      std::cout << J.transpose() << std::endl;
      std::cout << H << std::endl;
      std::cout << - H.ldlt().solve(J).transpose() << std::endl;
    }
//    std::cout << H << std::endl;
//    std::cout << "J " << J.transpose() << std::endl;
    J = - delta*H.ldlt().solve(J);
//    std::cout << "dx " << J.transpose() << std::endl;

    RPrev = R;
    tPrev = t;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%100==0)
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9) {
      std::cout << "@" << it << ": f=" << f << " f_prev=" << f_prev << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
      break;
    }
  }
  if (f_prev < f) {
    R = RPrev;
    t = tPrev;
  }
  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

  std::cout << "Using SO(3) formulation second order using SS -------------------"
    << std::endl;
  R = Rmu;
  t.fill(0);
  delta = 0.5;
  f_prev = 1e99;
  f = -tau_R_*(Rmu.transpose()*R).trace()
    + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
  for (uint32_t i=0; i<N; ++i) {
    const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
    f += 0.5*x.dot(covInv*x);
  }
  std::cout << "f=" << f << std::endl;
  tPrev = t;
  RPrev = R;
  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
    << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  for (uint32_t it=0; it<2000; ++it) {
    Eigen::Matrix<float,6,1> J = OdomJacobian(t, t_prev, R, Rmu);
    Eigen::Matrix<float,6,6> H = OdomHessian(t, t_prev, R, Rmu);
    for (uint32_t k=0; k<K; ++k) {
      const Eigen::Vector3f a = t-mus[k];
      AddClusterJacobian(ss[k],R,covLdlt,a,J);
      AddClusterHessian(ss[k],R,covInv,covLdlt,a,H);
    }
    if(it < 10) {
      std::cout << J.transpose() << std::endl;
      std::cout << H << std::endl;
      std::cout << - H.ldlt().solve(J).transpose() << std::endl;
    }
//    std::cout << H << std::endl;
//    std::cout << "J " << J.transpose() << std::endl;
    J = - delta*H.ldlt().solve(J);
//    std::cout << "dx " << J.transpose() << std::endl;

    RPrev = R;
    tPrev = t;

    t = t + J.topRows<3>();
    Eigen::Vector3f Jw = J.bottomRows<3>();
    SO3f R_(R);
    R = (R_ + Jw).matrix();

    f_prev = f;
    f = -tau_R_*(Rmu.transpose()*R).trace()
      + 0.5*(t - t_prev).dot(Sigma_t_.ldlt().solve(t-t_prev));
    for (uint32_t i=0; i<N; ++i) {
      const Eigen::Vector3f x = R*ps[i]+t-mus[zs[i]];
      f += 0.5*x.dot(covInv*x);
    }
    if (it%1==0) {
      std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
      std::cout << " -- d angle " << acos(std::min(1.,((R*R_true).trace()-1)*0.5))*180./M_PI
        << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
    }
    if ((f_prev - f)/fabs(f) < 1e-9) {
      std::cout << "@" << it << ": f=" << f << " f_prev=" << f_prev << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
      break;
    }
  }
  if (f_prev < f) {
    R = RPrev;
    t = tPrev;
  }
  std::cout << " -- d angle " << acos(((R*R_true).trace()-1)*0.5)*180./M_PI
            << " |dt| " << (t_true - (-R.transpose()*t)).sum() << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << "t " << t.transpose() << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "inverses" << std::endl;
  std::cout << R.transpose() << std::endl;
  std::cout << "t " << (-R.transpose()*t).transpose() << std::endl;
  std::cout << "true" << std::endl;
  std::cout << R_true << std::endl;
  std::cout << "t " << t_true.transpose() << std::endl;

}


#include <Eigen/Core>

#include <igl/readOBJ.h>
#include <igl/point_mesh_squared_distance.h>

extern "C" {

Eigen::MatrixXd V1, V2;
Eigen::MatrixXi F1, F2;
void LoadObj(const char* filename, int mesh_id) {
    if (mesh_id == 1)
        igl::readOBJ(filename, V1, F1);
    else
        igl::readOBJ(filename, V2, F2);
}

double OneWayChamferDistance(const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& F1,
    const Eigen::MatrixXd& V2,
    const Eigen::MatrixXi& F2,
    int resample_points) {
    Eigen::VectorXd sqrD;
    Eigen::VectorXi I;
    Eigen::MatrixXd C;
    if (resample_points) {

        std::vector<double> areas(F1.rows() + 1, 0);
        double total_area = 0;
        for (int i = 0; i < F1.rows(); ++i) {
            const Eigen::Vector3d& v1 = V1.row(F1(i, 0));
            const Eigen::Vector3d& v2 = V1.row(F1(i, 1));
            const Eigen::Vector3d& v3 = V1.row(F1(i, 2));
            double area = ((v2 - v1).cross(v3 - v1)).norm();
            areas[i + 1] = area;
            total_area += area;
        }
        for (int i = 1; i < areas.size(); ++i)
            areas[i] += areas[i - 1];

        Eigen::MatrixXd NV(resample_points, 3);
        for (int i = 0; i < resample_points; ++i) {
            double r = rand() / (double)RAND_MAX * areas.back();
            auto lower = std::lower_bound(areas.begin(), areas.end(), r) - 1;
            int tri_idx = lower - areas.begin();
            if (tri_idx < 0) {
                tri_idx = 0;
            }
            double u = rand() / (double)RAND_MAX;
            double v = rand() / (double)RAND_MAX;
            if (u + v > 1) {
                u = 1 - u;
                v = 1 - v;
            }
            auto v1 = V1.row(F1(tri_idx, 0));
            auto v2 = V1.row(F1(tri_idx, 1));
            auto v3 = V1.row(F1(tri_idx, 2));

            NV.row(i) = u * (v2 - v1) + v * (v3 - v1) + v1;
        }
        igl::point_mesh_squared_distance(NV,V2,F2,sqrD,I,C);
    } else {
        igl::point_mesh_squared_distance(V1,V2,F2,sqrD,I,C);
    }
    double distance = 0;
    for (int i = 0; i < sqrD.size(); ++i) {
        distance += sqrt(sqrD[i]);
    }
    double ans = distance / sqrD.size();
    return ans;
}

double OneWayChamfer(int src_id, int resample_points = 0) {
    if (src_id == 1) {
        return OneWayChamferDistance(V1, F1, V2, F2, resample_points);
    }
    return OneWayChamferDistance(V2, F2, V1, F1, resample_points);
}

double TwoWayChamfer(int resample_points = 0) {
    double distance1 = OneWayChamferDistance(V1, F1, V2, F2, resample_points);
    double distance2 = OneWayChamferDistance(V2, F2, V1, F1, resample_points);

    return distance1 + distance2;
}

void SetMesh(double* vertices, int* faces, int num_V, int num_F, int id) {
    auto &V = (id == 1) ? V1 : V2;
    auto &F = (id == 1) ? F1 : F2;

    V = Eigen::MatrixXd(num_V, 3);
    for (int i = 0; i < num_V; ++i) {
        V.row(i) = Eigen::Vector3d(vertices[i * 3],vertices[i * 3 + 1],vertices[i * 3 + 2]);
    }

    F = Eigen::MatrixXi(num_F, 3);
    for (int i = 0; i < num_F; ++i) {
        F.row(i) = Eigen::Vector3i(faces[i * 3], faces[i * 3 + 1], faces[i * 3 + 2]);
    }
}

double AreaRatio(const char* f1, const char* f2) {
    LoadObj(f1, 2);
    LoadObj(f2, 1);
    {
        Eigen::MatrixXd V2_buf(V2.rows() * 2, 3);
        Eigen::MatrixXi F2_buf(F2.rows() * 2, 3);
        for (int i = 0; i < V2.rows(); ++i) {
            V2_buf.row(i) = V2.row(i);
            V2_buf.row(i + V2.rows()) = V2.row(i);
            V2_buf(i + V2.rows(), 0) = -V2_buf(i + V2.rows(), 0);
        }
        for (int i = 0; i < F2.rows(); ++i) {
            F2_buf.row(i) = F2.row(i);
            F2_buf.row(i + F2.rows()) = F2.row(i);
            for (int j = 0; j < 3; ++j)
                F2_buf(i + F2.rows(), j) += V2.rows();
        }
        F2 = F2_buf;
        V2 = V2_buf;
    }
    {
        Eigen::VectorXd sqrD;
        Eigen::VectorXi I;
        Eigen::MatrixXd C;
        std::vector<double> areas(F1.rows() + 1, 0);
        double total_area = 0;
        for (int i = 0; i < F1.rows(); ++i) {
            const Eigen::Vector3d& v1 = V1.row(F1(i, 0));
            const Eigen::Vector3d& v2 = V1.row(F1(i, 1));
            const Eigen::Vector3d& v3 = V1.row(F1(i, 2));
            double area = ((v2 - v1).cross(v3 - v1)).norm();
            areas[i + 1] = area;
            total_area += area;
        }
        for (int i = 1; i < areas.size(); ++i)
            areas[i] += areas[i - 1];

        int resample_points = 100000;
        Eigen::MatrixXd NV(resample_points, 3);
        for (int i = 0; i < resample_points; ++i) {
            double r = rand() / (double)RAND_MAX * areas.back();
            auto lower = std::lower_bound(areas.begin(), areas.end(), r) - 1;
            int tri_idx = lower - areas.begin();
            if (tri_idx < 0) {
                tri_idx = 0;
            }
            double u = rand() / (double)RAND_MAX;
            double v = rand() / (double)RAND_MAX;
            if (u + v > 1) {
                u = 1 - u;
                v = 1 - v;
            }
            auto v1 = V1.row(F1(tri_idx, 0));
            auto v2 = V1.row(F1(tri_idx, 1));
            auto v3 = V1.row(F1(tri_idx, 2));

            NV.row(i) = u * (v2 - v1) + v * (v3 - v1) + v1;
        }

        igl::point_mesh_squared_distance(NV,V2,F2,sqrD,I,C);
        int inliers = 0;
        for (int i = 0; i < sqrD.size(); ++i) {
            double dis = sqrt(sqrD[i]);
            if (dis < 0.03)
                inliers += 1;
        }
        return (double)inliers / sqrD.size();
    }
}

};


#include <boost/math/distributions/normal.hpp>


// Filename: imf_example.cpp (part of MTL4)

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

int main(int, char** argv)
{
    typedef double value_type;

    std::string program_dir= mtl::io::directory_name(argv[0]),
  	        matrix_file= mtl::io::join(program_dir, "../../mtl/test/matrix_market/square3.mtx");
    //define and read element structure
    mtl::mat::element_structure<value_type> A;
    read_el_matrix(matrix_file, A);

    int size= int( num_cols(A) );
    mtl::dense_vector<value_type>    x(size, 1), b( A * x );

    //assemble sparse matrix from element structure (unnecessary)
    mtl::compressed2D<value_type> B;
    assemble_compressed(A, B);

    //create imf preconditioner with 3 levels of fill-in
    itl::pc::imf_preconditioner<value_type> precond(A, 3);

    itl::cyclic_iteration<value_type>    iterA(b, size, 1.e-8, 0.0, 5),
					  iterB(b, size, 1.e-8, 0.0, 5);
    x= 0;
    bicgstab(A, x, b, precond, iterA);  //solve A*x=b  with element_structure A
    x= 0;
    bicgstab(B, x, b, precond, iterB);  //solve A*x=b  with assembled sparse matrix B

    return 0;
}


//=============================================================================
//
//  CLASS ConeConstraint
//
//=============================================================================

#ifndef COMISO_CONECONSTRAINT_HH
#define COMISO_CONECONSTRAINT_HH

//== INCLUDES =================================================================

#include <CoMISo/Config/CoMISoDefines.hh>
#include "NConstraintInterface.hh"
#include <Eigen/StdVector>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

class COMISODLLEXPORT ConeConstraint : public NConstraintInterface
{
public:

  // sparse vector type
  typedef NConstraintInterface::SVectorNC SVectorNC;
  typedef NConstraintInterface::SMatrixNC SMatrixNC;

  /// Default constructor
  ConeConstraint();

  // cone constraint of the form -> 0.5*(c_ * x(i_)^2 - x^T Q_ x) >= 0
  ConeConstraint(const double _c, const int _i, const SMatrixNC& _Q);

  virtual int n_unknowns();

  // resize coefficient vector = #unknowns
  void  resize(const unsigned int _n);

  // clear to zero constraint 0 =_type 0
  void  clear();

  const double&    c() const;
        double&    c();

  const int&       i() const;
        int&       i();

  const SMatrixNC& Q() const;
        SMatrixNC& Q();

  virtual double eval_constraint ( const double* _x );

  virtual void eval_gradient( const double* _x, SVectorNC& _g      );

  virtual void eval_hessian    ( const double* _x, SMatrixNC& _h      );

  virtual bool   is_linear()         const { return false;}
  virtual bool   constant_gradient() const { return false;}
  virtual bool   constant_hessian () const { return true;}

private:

  // cone constraint of the form -> 0.5*(c_ * x(i_)^2 - x^T Q_ x) >= 0
  double    c_;
  int       i_;
  SMatrixNC Q_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================
// support std vectors
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(COMISO::ConeConstraint);
//=============================================================================
#endif // ACG_CONECONSTRAINT_HH defined
//=============================================================================


// // [[Rcpp::depends(cgal4h)]]
// #include <Rcpp.h>
//
// #include <CGAL/Cartesian.h>
// #include <CGAL/MP_Float.h>
// #include <CGAL/Quotient.h>
// #include <CGAL/Arr_linear_traits_2.h>
// #include <CGAL/Arr_segment_traits_2.h>
// #include <CGAL/Arrangement_2.h>
// // typedef CGAL::Quotient<CGAL::MP_Float> Number_type;
// // typedef CGAL::Cartesian<Number_type> Kernel;
// // // typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
// // typedef CGAL::Arr_linear_traits_2<Kernel> Traits_2;
// // typedef Traits_2::Point_2 Point_2;
// // typedef Traits_2::X_monotone_curve_2 Segment_2;
// // typedef CGAL::Arrangement_2<Traits_2> Arrangement_2;
// // #include <CGAL/Cartesian.h>
// #include <CGAL/Exact_rational.h>
// #include <CGAL/Arr_segment_traits_2.h>
// #include <CGAL/Arr_extended_dcel.h>
// #include <CGAL/Arrangement_2.h>
// #include <CGAL/graph_traits_dual_arrangement_2.h>
// #include <CGAL/Arr_face_index_map.h>
// #include <climits>
// #include <boost/graph/breadth_first_search.hpp>
// #include <boost/graph/visitors.hpp>
//
// // A property map that reads/writes the information to/from the extended
// // face.
// template <typename Arrangement, class Type> class Extended_face_property_map {
// public:
//   typedef typename Arrangement::Face_handle       Face_handle;
//   // Boost property type definitions.
//   typedef boost::read_write_property_map_tag      category;
//   typedef Type                                    value_type;
//   typedef value_type&                             reference;
//   typedef Face_handle                             key_type;
//   // The get function is required by the property map concept.
//   friend reference get(const Extended_face_property_map& /* map */,
//                        key_type key)
//   { return key->data(); }
//   // The put function is required by the property map concept.
//   friend void put(Extended_face_property_map /* map */,
//                   key_type key, value_type val)
//   { key->set_data(val); }
// };
//
//
//
// typedef CGAL::Cartesian<CGAL::Exact_rational>                Kernel;
// typedef CGAL::Arr_segment_traits_2<Kernel>                   Traits_2;
// typedef CGAL::Arr_face_extended_dcel<Traits_2, unsigned int> Dcel;
// typedef CGAL::Arrangement_2<Traits_2, Dcel>                  Ex_arrangement;
// typedef CGAL::Dual<Ex_arrangement>                           Dual_arrangement;
// typedef CGAL::Arr_face_index_map<Ex_arrangement>             Face_index_map;
// typedef Extended_face_property_map<Ex_arrangement,unsigned int>
//                                                              Face_property_map;
// typedef Kernel::Point_2                                      Point_2;
// typedef Kernel::Segment_2                                    Segment_2;
//
//
//
// // void print_ccb (Arrangement_2::Ccb_halfedge_const_circulator circ) {
// //   Arrangement_2::Ccb_halfedge_const_circulator curr = circ;
// //   Rcpp::Rcout << "(" << curr->source()->point() << ")";
// //   do {
// //     Arrangement_2::Halfedge_const_handle he = curr->handle();
// //     Rcpp::Rcout << " [" << he->curve() << "] "
// //               << "(" << he->target()->point() << ")";
// //   } while (++curr != circ);
// //   Rcpp::Rcout << std::endl;
// // }
//
//
// // void print_face (Arrangement_2::Face_const_handle f) {
// //   // Print the outer boundary.
// //   if (f->is_unbounded())
// //     Rcpp::Rcout << "Unbounded face. " << std::endl;
// //   // else {
// //   //   Rcpp::Rcout << "Outer boundary: ";
// //   //   print_ccb (f->outer_ccb());
// //   // }
// //   // Print the boundary of each of the holes.
// //   // Arrangement_2::Hole_const_iterator hi;
// //   int index = 1;
// //   // for (hi = f->holes_begin(); hi != f->holes_end(); ++hi, ++index) {
// //   //   Rcpp::Rcout << " Hole #" << index << ": ";
// //   //   print_ccb (*hi);
// //   // }
// //   // Print the isolated vertices.
// //   Arrangement_2::Isolated_vertex_const_iterator iv;
// //   for (iv = f->isolated_vertices_begin(), index = 1;
// //        iv != f->isolated_vertices_end(); ++iv, ++index){
// //     Rcpp::Rcout << " Isolated vertex #" << index << ": "
// //               << "(" << iv->point() << ")" << std::endl;
// //   }
// // }
//
//
//
// // [[Rcpp::export]]
// Rcpp::NumericVector construct_arrangement(Rcpp::NumericVector x) {
// // 	Arrangement_2 arr;
// //   Segment_2 cv[3];
// //   Point_2 p1 (0, 0), p2 (0, 4), p3 (4, 0);
// //   cv[0] = Segment_2 (p1, p2);
// //   cv[1] = Segment_2 (p2, p3);
// //   cv[2] = Segment_2 (p3, p1);
// //   CGAL::insert (arr, &cv[0], &cv[3]);
// //   // ArrangementDcel::Face_const_iterator fit;
// //   Rcpp::Rcout << arr.number_of_faces() << " faces:" << std::endl;
// //   for (auto fit = arr.faces_begin(); fit != arr.faces_end(); ++fit)
// //     print_face (fit);
// //   return x * 2;
// // Construct an arrangement of seven intersecting line segments.
//   Point_2 p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
//   Ex_arrangement  arr;
//   insert(arr, Segment_2(p1, p6));
//   insert(arr, Segment_2(p1, p4));  insert(arr, Segment_2(p2, p6));
//   insert(arr, Segment_2(p3, p7));  insert(arr, Segment_2(p3, p5));
//   insert(arr, Segment_2(p6, p7));  insert(arr, Segment_2(p4, p7));
//   // Create a mapping of the arrangement faces to indices.
//   Face_index_map  index_map(arr);
//   // Perform breadth-first search from the unbounded face, using the event
//   // visitor to associate each arrangement face with its discover time.
//   unsigned int    time = 0;
//   boost::breadth_first_search(Dual_arrangement(arr), arr.unbounded_face(),
//                               boost::vertex_index_map(index_map).visitor
//                               (boost::make_bfs_visitor
//                                (stamp_times(Face_property_map(), time,
//                                             boost::on_discover_vertex()))));
//   // Print the discover time of each arrangement face.
//   Ex_arrangement::Face_iterator  fit;
//   for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
//     std::cout << "Discover time " << fit->data() << " for ";
//     if (fit != arr.unbounded_face()) {
//       std::cout << "face ";
//       // print_ccb<Ex_arrangement>(fit->outer_ccb());
//     }
//     else std::cout << "the unbounded face." << std::endl;
//   }
// 	//arr.unbounded_faces_begin()
// }
//
//
// /*** R
// construct_arrangement(42)
// */


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse sine in \f$\pi\f$ multiples.

    @par Header <boost/simd/function/asinpi.hpp>

    @see asin, asind, sinpi

    @par Example:

      @snippet asinpi.cpp asinpi

    @par Possible output:

      @snippet asinpi.txt asinpi

  **/
  IEEEValue asinpi(IEEEValue const & x);
} }
#endif

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

#endif


#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>

namespace py = pybind11;
using namespace std;

#include <Eigen/Dense>

// https://pybind11.readthedocs.io/en/stable/advanced/cast/eigen.html

class MyClass
{
private:
    Eigen::MatrixXd ary;

public:
    Eigen::MatrixXd &ones(int, int);
};

Eigen::MatrixXd &MyClass::ones(int ny, int nx)
{
    ary = Eigen::MatrixXd::Zero(ny, nx);

    for (int y = 0; y < ny; ++y)
    {
        for (int x = 0; x < nx; ++x)
        {
            ary(y, x) = 1;
        }
    }

    return ary;
}

py::array_t<int> ones(int ny, int nx)
{
    vector<int> arraysize{ny, nx};
    py::array_t<int> ary{arraysize};

    for (int y = 0; y < ny; ++y)
    {
        for (int x = 0; x < nx; ++x)
        {
            *ary.mutable_data(y, x) = 1;
        }
    }

    return ary;
}

py::tuple oneses(int ny0, int nx0, int ny1, int nx1)
{

    return py::make_tuple(ones(ny0, nx0), ones(ny1, nx1));
}

PYBIND11_MODULE(numpytest, m)
{
    m.doc() = "pybind11 example plugin";
    m.def("ones", &ones, "A function which returns ones array");
    m.def("oneses", &oneses, "A function which returns ones array x2");

    py::class_<MyClass>(m, "MyClass")
        .def(py::init<>())
        .def("ones", &MyClass::ones, py::return_value_policy::reference_internal); // reference
    //        .def("ones", &MyClass::ones); //copy
}


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/tenpower.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/oneo_10.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/three.hpp>
#include <boost/simd/include/constants/hundred.hpp>
#include <boost/simd/include/functions/sqr.hpp>
#include <boost/simd/include/functions/splat.hpp>

NT2_TEST_CASE_TPL ( tenpower_unsigned_int, BOOST_SIMD_SIMD_UINT_CONVERT_TYPES)
{

  using boost::simd::tenpower;
  using boost::simd::tag::tenpower_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<tenpower_(vT)>::type r_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::One<vT>()), boost::simd::Ten<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Two<vT>()), boost::simd::Hundred<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Zero<vT>()), boost::simd::One<r_t>(), 0.5);
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( tenpower_signed_int,  BOOST_SIMD_SIMD_INT_CONVERT_TYPES)
{

  using boost::simd::tenpower;
  using boost::simd::tag::tenpower_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<tenpower_(vT)>::type r_t;
  // specific values tests
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Mone<vT>()), boost::simd::Oneo_10<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::One<vT>()), boost::simd::Ten<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Two<vT>()), boost::simd::Hundred<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Three<vT>()),boost::simd::splat<r_t>(1000.0), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Mtwo<vT>()), boost::simd::sqr(boost::simd::Oneo_10<r_t>()), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Zero<vT>()), boost::simd::One<r_t>(), 0.5);
} // end of test for signed_int_


#define BOOST_TEST_MODULE pcraster com statistics
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include <cmath>
#include "com_algorithm.h"
#include "com_statistics.h"

// KDJ, 20150928: Some tests used to be excluded on certain platforms:

// #ifndef __x86_64__
//   // bugzilla #80
//   suite->add(BOOST_CLASS_TEST_CASE(&StatisticsTest::testVariance1, instance));
//   suite->add(BOOST_CLASS_TEST_CASE(&StatisticsTest::testVariance2, instance));
//   suite->add(BOOST_CLASS_TEST_CASE(&StatisticsTest::testStandardDeviation, instance));
//   suite->add(BOOST_CLASS_TEST_CASE(&StatisticsTest::testPercentile, instance));
// #else
//   suite->add(BOOST_CLASS_TEST_CASE(&StatisticsTest::testSuse, instance));
// #endif

namespace com {
 namespace statisticsTest {
    struct Reverse {
      bool operator()(const double& e1, const double& e2) const
      {
        return e1>e2;
      }
    };
 }
}

BOOST_AUTO_TEST_CASE(sum)
{
  using namespace com;

  int values[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
  const size_t nrValues = ARRAY_SIZE(values);

  Sum<int> sum = std::for_each(values, values + nrValues, Sum<int>());

  BOOST_CHECK(sum == 55);
  BOOST_CHECK(sum.sum() == 55);
}

BOOST_AUTO_TEST_CASE(sum_nr)
{
  using namespace com;

  int values[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
  const size_t nrValues = ARRAY_SIZE(values);
  SumNr<int> sum = std::for_each(values, values + nrValues, SumNr<int>());

  BOOST_CHECK(sum.sum() == 55);
  BOOST_CHECK(sum.nr() ==  nrValues);
}

BOOST_AUTO_TEST_CASE(average)
{
  using namespace com;

  double values[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
  const size_t nrValues = ARRAY_SIZE(values);
  Average<> a = std::for_each(values, values + nrValues,
      Average<>());

  BOOST_CHECK(a.sum() == 55);
  BOOST_CHECK(a.nr() ==  nrValues);
  BOOST_CHECK(a.average() ==  5.5);

  Average<double> empty;
  BOOST_CHECK(empty.nr()==0);
  BOOST_CHECK(empty.average(-1)==-1);
}

BOOST_AUTO_TEST_CASE(average_min_max)
{
  using namespace com;

  {
    double values[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
    const size_t nrValues = ARRAY_SIZE(values);
    AverageMinMax<double> a = std::for_each(values, values + nrValues,
        AverageMinMax<double>());

    BOOST_CHECK(a.sum() == 55);
    BOOST_CHECK(a.nr() ==  nrValues);
    BOOST_CHECK(a.average() ==  5.5);
    BOOST_CHECK(a.minimum() ==  1);
    BOOST_CHECK(a.maximum() ==  10);
  }
  {
    double values[] = { 6, 2, 10, 4, 5, 1, 7, 8, 9, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    AverageMinMax<double> a = std::for_each(values, values + nrValues,
        AverageMinMax<double>());

    BOOST_CHECK(a.sum() == 55);
    BOOST_CHECK(a.nr() ==  nrValues);
    BOOST_CHECK(a.average() ==  5.5);
    BOOST_CHECK(a.minimum() ==  1);
    BOOST_CHECK(a.maximum() ==  10);
  }
}

BOOST_AUTO_TEST_CASE(variance_1)
{
  using namespace com;

  // Empty collection.
  {
    double *values = 0;
    const size_t nrValues = 0u;  // ARRAY_SIZE requires non-empty array.
    Variance1<double> variance = std::for_each(values, values + nrValues,
                   Variance1<double>(0.0));
    BOOST_CHECK(variance == 0.0);
  }

  // Collection with equal values.
  {
    double values[] = { 3, 3, 3, 3, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    Variance1<double> variance = std::for_each(values, values + nrValues,
                   Variance1<double>(3.0));
    BOOST_CHECK(variance == 0.0);
  }

  // Collection with different values.
  {
    double values[] = { 2, 2, 2, 2, 2.5, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    Variance1<double> variance = std::for_each(values, values + nrValues,
                   Variance1<double>(2.5));
    BOOST_CHECK(variance == 0.25);
  }
}

BOOST_AUTO_TEST_CASE(variance_2)
{
  using namespace com;

  // Empty collection.
  {
    double *values = 0;
    const size_t nrValues = 0u;  // ARRAY_SIZE requires non-empty array.
    Variance2<double> variance = std::for_each(values, values + nrValues,
                   Variance2<double>());
    BOOST_CHECK(variance == 0.0);
  }

  // Collection with equal values.
  {
    double values[] = { 3, 3, 3, 3, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    Variance2<double> variance = std::for_each(values, values + nrValues,
                   Variance2<double>());
    BOOST_CHECK(variance == 0.0);
  }

  // Collection with different values.
  {
    double values[] = { 2, 2, 2, 2, 2.5, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    Variance2<double> variance = std::for_each(values, values + nrValues,
                   Variance2<double>());
    BOOST_CHECK(variance == 0.25);
  }
}

BOOST_AUTO_TEST_CASE(standard_deviation)
{
  using namespace com;

  // Empty collection.
  {
    double *values = 0;
    const size_t nrValues = 0u;  // ARRAY_SIZE requires non-empty array.
    StandardDeviation<double> stdDev = std::for_each(values,
                   values + nrValues, StandardDeviation<double>());
    BOOST_CHECK(stdDev == 0.0);
  }

  // Collection with equal values.
  {
    double values[] = { 3, 3, 3, 3, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    StandardDeviation<double> stdDev = std::for_each(values, values + nrValues,
                   StandardDeviation<double>());
    BOOST_CHECK(stdDev == 0.0);
  }

  // Collection with different values.
  {
    double values[] = { 2, 2, 2, 2, 2.5, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    StandardDeviation<double> stdDev = std::for_each(values, values + nrValues,
                   StandardDeviation<double>());
    BOOST_CHECK(stdDev == 0.5);
  }
}

BOOST_AUTO_TEST_CASE(average_sd_min_max)
{
  using namespace com;

  // Collection with different values.
  {
    double values[] = { 2, 2, 2, 2, 2.5, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    AverageSdMinMax<double> asmm = std::for_each(values, values + nrValues,
                   AverageSdMinMax<double>());
    BOOST_CHECK(asmm.sd() == 0.5);
    BOOST_CHECK(asmm.average() == 2.5);
    BOOST_CHECK(asmm.minimum() == 2.0);
    BOOST_CHECK(asmm.maximum() == 3.0);
  }

  // Empty collection.
  {

    double values[1] = { 99999 };
    AverageSdMinMax<double> asmm = std::for_each(values, values+0,
                   AverageSdMinMax<double>());
    BOOST_CHECK(asmm.sd() == 0.0);
    BOOST_CHECK(asmm.nr() == 0);
    BOOST_CHECK(asmm.average(5) == 5);
   }

  // Collection with equal values.
  {
    double values[] = { 3, 3, 3, 3, 3, 3, 3, 3 };
    const size_t nrValues = ARRAY_SIZE(values);
    AverageSdMinMax<double> asmm = std::for_each(values, values + nrValues,
                   AverageSdMinMax<double>());
    BOOST_CHECK(asmm.sd() == 0.0);
    BOOST_CHECK(asmm.average() == 3);
    BOOST_CHECK(asmm.minimum() == 3);
    BOOST_CHECK(asmm.maximum() == 3);
  }
}

BOOST_AUTO_TEST_CASE(percentile_)
{
#ifndef __x86_64__
  using namespace com;

  typedef std::vector<double> T;
  typedef T::iterator         I;
 {
  double data[]={1,3,2,5,4,9,8,0,6,7};
  T val;
  std::copy(data,data+ARRAY_SIZE(data),std::back_inserter(val));

  // out of range
  BOOST_CHECK(percentile<I>(val.begin(),val.end(),5) == val.end());
  BOOST_CHECK(percentile<I>(val.begin(),val.end(),-123) == val.end());

  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0) == 0);
  BOOST_CHECK( percentile<I>(val.begin(),val.end(),0) == val.begin());
  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0.005) == 0);
  BOOST_CHECK( percentile<I>(val.begin(),val.end(),0.005) == val.begin());

  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),1) == val.size()-1);
  BOOST_CHECK( percentile<I>(val.begin(),val.end(),1) == val.end()-1);
  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0.9994) == val.size()-1);
  BOOST_CHECK( percentile<I>(val.begin(),val.end(),0.9994) == val.end()-1);

#ifdef _MSC_VER
  // bugzilla #80
  BOOST_WARN(*percentile<I>(val.begin(),val.end(),0.6)  == 5);
#else
  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0.6)  == 5);
#endif

  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0.47) == 4);

  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0,
                         statisticsTest::Reverse())  == 9);
  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),1,
                         statisticsTest::Reverse())  == 0);

#ifdef _MSC_VER
  // bugzilla #80
  BOOST_WARN(*percentile<I>(val.begin(),val.end(),0.6,
                         statisticsTest::Reverse())  == 4);
#else
  BOOST_CHECK(*percentile<I>(val.begin(),val.end(),0.6,
                         statisticsTest::Reverse())  == 4);
#endif
 }
 {
  T e;
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0)      == e.end());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0.005)  == e.end());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),1)      == e.end());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0.9994) == e.end());
 }
 {
  T e;
  e.push_back(4);
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0)      == e.begin());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0.005)  == e.begin());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0.5)    == e.begin());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),1)      == e.begin());
  BOOST_CHECK( percentile<I>(e.begin(),e.end(),0.9994) == e.begin());
  BOOST_CHECK(*percentile<I>(e.begin(),e.end(),0)      == 4);
  BOOST_CHECK(*percentile<I>(e.begin(),e.end(),0.005)  == 4);
  BOOST_CHECK(*percentile<I>(e.begin(),e.end(),1)      == 4);
  BOOST_CHECK(*percentile<I>(e.begin(),e.end(),0.9994) == 4);
 }
#endif
}

BOOST_AUTO_TEST_CASE(suse)
{
  using namespace com;

  bool suse64FailsALot=false;
  BOOST_WARN(suse64FailsALot);
}


// This program tests the functionality of my_vector_tools (parallel
// and serial projection on FE spaces).

// Deal.ii MPI
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/function.h>
#include <deal.II/base/index_set.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/mpi.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/tensor.h>
#include <deal.II/base/tensor_function.h>
#include <deal.II/base/utilities.h>
#include <deal.II/distributed/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_nedelec.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/vector.h>

// C++ STL
#include <iostream>

// My library
#include <vector_tools/my_vector_tools.h>
#include <vector_tools/my_vector_tools.tpp>

using namespace dealii;

///////////////////////////////////
///////////////////////////////////
template <int dim>
class MyVectorFunction : public TensorFunction<1, dim>
{
public:
  MyVectorFunction()
    : TensorFunction<1, dim>(){};

  void
    value_list(const std::vector<Point<dim>> &points,
               std::vector<Tensor<1, dim>> &  values) const override;
};

template <int dim>
void
  MyVectorFunction<dim>::value_list(const std::vector<Point<dim>> &points,
                                    std::vector<Tensor<1, dim>> &  values) const
{
  Assert(points.size() == values.size(),
         ExcDimensionMismatch(points.size(), values.size()));

  for (unsigned int i = 0; i < values.size(); ++i)
    {
      values[i].clear();
      for (unsigned int d = 0; d < dim; ++d)
        values[i][d] = d + 1.0;
    }
}
///////////////////////////////////
///////////////////////////////////

///////////////////////////////////
///////////////////////////////////
int
  main(int argc, char *argv[])
{
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization(
    argc, argv, dealii::numbers::invalid_unsigned_int);

  MPI_Comm mpi_communicator(MPI_COMM_WORLD);

  const int dim = 3, degree = 0, n_refine = 3;

  ConditionalOStream pcout(std::cout,
                           (Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) ==
                            0));

  parallel::distributed::Triangulation<dim> triangulation(
    mpi_communicator,
    typename Triangulation<dim>::MeshSmoothing(
      Triangulation<dim>::smoothing_on_refinement |
      Triangulation<dim>::smoothing_on_coarsening));

  GridGenerator::hyper_cube(triangulation, 0.0, 1.0, true);
  triangulation.refine_global(n_refine);

  FE_Nedelec<dim> fe(degree);

  DoFHandler<dim> dof_handler(triangulation);
  dof_handler.distribute_dofs(fe);

  AffineConstraints<double> constraints;
  constraints.clear();
  DoFTools::make_hanging_node_constraints(dof_handler, constraints);
  constraints.close();

  // Quadrature used for projection
  QGauss<dim> quad_rule(/* order = */ 3);

  // Setup function
  MyVectorFunction<dim> my_vector_function;

  TrilinosWrappers::MPI::Vector projected_function;

  IndexSet locally_owned_dofs;
  locally_owned_dofs = dof_handler.locally_owned_dofs();
  projected_function.reinit(locally_owned_dofs, mpi_communicator);

  try
    {
      MyVectorTools::project_on_fe_space(dof_handler,
                                         constraints,
                                         quad_rule,
                                         my_vector_function,
                                         projected_function,
                                         mpi_communicator);

      // Write only for process 0
      if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
        {
          IndexSet::ElementIterator local_index = locally_owned_dofs.begin(),
                                    local_index_end = locally_owned_dofs.end();

          std::cout << "Values of the projected vector in MPI process 0:"
                    << std::endl;

          for (; local_index != local_index_end; ++local_index)
            {
              std::cout << "   " << projected_function[*local_index]
                        << std::endl;
            }

          std::cout << "Projection test succeeded." << std::endl;
        }

      constraints.clear();
      dof_handler.clear();
    }
  catch (...)
    {
      //      if (Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
      std::cout << "Projection test failed." << std::endl;
    }
}
///////////////////////////////////
///////////////////////////////////


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

BZ_USING_NAMESPACE(blitz)

int main()
{
Array<float,2> test(8,8), test2(5,5) ;

test = 5;

Range I(2,6) ;
Range J(3,7) ;

// Koenig lookup hack
#if defined(__GNUC__) && (__GNUC__ < 3)
test2 = where(blitz::operator> (test(I,J), test(I-1,J)), 0, test(I,J));
#else
test2 = where(test(I,J) > test(I-1,J), 0, test(I,J));
#endif

BZTEST(test2(3,3) == 5);

cout << test2 << endl ;

}


#include <boost/multiprecision/gmp.hpp>
#include <iostream>
#include <stdio.h>
#include <string.h>

void findy(mpf_t y, long unsigned int nn, int precision)
{
	mpz_t n,n1,y1,y2;
	mpz_inits(n,n1,y1,y2,NULL);

	mpz_set_ui( n,  nn);
	mpz_set_ui(n1,nn+1);

	mpz_pow_ui(y1,n1,nn+1);
	mpz_pow_ui(y2,n ,nn+1);

	mpf_t y3,y4;
	mpf_init2(y3,precision);
	mpf_init2(y4,precision);
	mpf_set_z(y3,y1);
	mpf_set_z(y4,y2);

	mpf_div(y,y3,y4);

	mpz_clear(n);
	mpz_clear(n1);
	mpz_clear(y1);
	mpz_clear(y2);
	mpf_clear(y3);
	mpf_clear(y4);
}

void findx(mpf_t x, long unsigned int nn, int precision)
{
	mpz_t n,n1,x1,x2;
	mpz_inits(n,n1,x1,x2,NULL);

	mpz_set_ui( n,  nn);
	mpz_set_ui(n1,nn+1);

	mpz_pow_ui(x1,n1,nn);
	mpz_pow_ui(x2,n ,nn);

	/*
	std::cout << "X1 = ";
	mpz_out_str(stdout, 10, x1);
	std::cout << std::endl;

	std::cout << "X2 = ";
	mpz_out_str(stdout, 10, x2);
	std::cout << std::endl;
	*/
	mpf_t x3,x4;
	mpf_init2(x3,precision);
	mpf_init2(x4,precision);
	mpf_set_z(x3,x1);
	mpf_set_z(x4,x2);

	mpf_div(x,x3,x4);

	mpz_clear(n);
	mpz_clear(n1);
	mpz_clear(x1);
	mpz_clear(x2);
	mpf_clear(x3);
	mpf_clear(x4);
}

int getsizef(mpf_t x)
{
	mp_exp_t exponent;
	char* c = mpf_get_str(NULL,&exponent,10,0,x);

	return strlen(c);
}

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

	int precision = 262144;
	char* c;

	mpf_t x,y;
	mpf_init2(x,precision);
	mpf_init2(y,precision);
	mp_exp_t exponent;

	for(long unsigned int nn = 1; nn<=5000; nn++)
	{
		findx(x,nn,precision);
		findy(y,nn,precision);

		//std::cout << "X = ";
		//mpf_out_str(stdout, 10, 10, x);
		//std::cout << std::endl;
		//int sz = getsizef(x);
		c = mpf_get_str(NULL,&exponent,10,0,x);
		int sz = strlen(c);
		if(sz < 78911)//315651)	//<precision/3.33
		{
			std::cout << "N = " << nn << "\t" << "# of digits of X = " << sz << " : ";// << std::endl;
			if(sz > 7) for(int i=0; i<10; i++) std::cout << c[sz-10+i];
			std::cout << "\t";

			bool ok = true;
			for (int i=sz-8; i<sz; i++)
				for (int j= i+1; j<sz; j++)
					if(c[i] == c[j]) ok = false;

			if(ok) std::cout << "OK!!!!!";
			else std::cout << "NO";
			std::cout << std::endl;
		}

		c = mpf_get_str(NULL,&exponent,10,0,y);
		sz = strlen(c);
		if(sz < 78911)
		{
			std::cout << "N = " << nn << "\t" << "# of digits of Y = " << sz << " : ";// << std::endl;
			if(sz > 7) for(int i=0; i<10; i++) std::cout << c[sz-10+i];
			std::cout << "\t";

			bool ok = true;
			for (int i=sz-8; i<sz; i++)
				for (int j= i+1; j<sz; j++)
					if(c[i] == c[j]) ok = false;

			if(ok) std::cout << "OK!!!!!";
			else std::cout << "NO";
			std::cout << std::endl;
		}

	}

	mpf_clear(x);

	return 0;
}


#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <boost/python.hpp>
#include <math.h>
#include <chrono>
#include <boost/python/numpy.hpp>

using namespace std;
using namespace boost::python;
using namespace Eigen;
namespace np = boost::python::numpy;

typedef vector<double> vec;
typedef vector<vec> mat;

#define PI 3.14159265

class Arm {
public:
  int numDOF;
  boost::python::list displacements;
  boost::python::list axes;
  boost::python::list rotOffsets;
  boost::python::tuple dispOffset;
  boost::python::list velocity_limits;
  boost::python::list joint_limits;
  char* name;
  Matrix3d rotX;
  Matrix3d rotY;
  Matrix3d rotZ;

  Arm(boost::python::list axes, boost::python::list displacements,
    boost::python::list rotOffsets, boost::python::tuple dispOffset, char* name) {
    Py_Initialize();
    np::initialize();
    this->axes = axes;
    this->displacements = displacements;
    this->rotOffsets = rotOffsets;
    this->dispOffset = dispOffset;
    this->numDOF = len(displacements);
    this->name = name;

    this->rotX = MatrixXd::Zero(3,3);
    this->rotY = MatrixXd::Zero(3,3);
    this->rotZ = MatrixXd::Zero(3,3);

    this->rotX(0,0) = 1.0;
    this->rotY(1,1) = 1.0;
    this->rotZ(2,2) = 1.0;
  }

  Matrix3d& rot3(char axis, double s, double c) {
    if (axis == 'z' || axis == 'Z') {
      this->rotZ(0,0) = c;
      this->rotZ(0,1) = -s;
      this->rotZ(1,0) = s;
      this->rotZ(1,1) = c;
      return this->rotZ;
    }
    else if(axis == 'y' || axis == 'Y') {
      this->rotY(0,0) = c;
      this->rotY(0,2) = s;
      this->rotY(2,0) = -s;
      this->rotY(2,2) = c;
      return this->rotY;
    }
    else if(axis == 'x' || axis == 'X') {
      this->rotX(1,1) = c;
      this->rotX(1,2) = -s;
      this->rotX(2,1) = s;
      this->rotX(2,2) = c;
      return this->rotX;
    }
    else {
      cout << "ERROR: not a valid axis label";
    }
  }

  boost::python::list getFrames(boost::python::list state) {
    boost::python::list ret;
    boost::python::list pts;
    Vector3d pt = this->array(this->dispOffset);
    pts.append(this->tolist(pt));
    boost::python::list frames;
    Matrix3d rot = MatrixXd::Zero(3,3);
    rot(0,0) = 1.0;
    rot(1,1) = 1.0;
    rot(2,2) = 1.0;
    frames.append(np::array(this->tolist(rot)));

    for(int i = 0; i < this->numDOF; i++) {
      double s = sin(extract<double>(state[i]));
      double c = cos(extract<double>(state[i]));

      //TODO: do rotation offsets

      char curr_axis = extract<char>(this->axes[i]);

      Matrix3d rmat = this->rot3(curr_axis,s,c);
      rot = rot*rmat;
      Vector3d disp;
      disp(0) = extract<double>((this->displacements[i])[0]);
      disp(1) = extract<double>((this->displacements[i])[1]);
      disp(2) = extract<double>((this->displacements[i])[2]);
      // cout << rot(1,1);
      // cout << "\n";
      // high_resolution_clock::time_point t1 = high_resolution_clock::now();
      pt = (rot*disp) + pt;
      // high_resolution_clock::time_point t2 = high_resolution_clock::now();
      // duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
      // std::cout << "It took me " << time_span.count() << " seconds.\n";

      pts.append(np::array(this->tolist(pt)));
      frames.append(np::array(this->tolist(rot)));
    }

    ret.append(pts);
    ret.append(frames);
    return ret;
  }

private:

  Vector3d array(boost::python::list input) {
    Vector3d v;
    v(0) = extract<double>(input[0]);
    v(1) = extract<double>(input[1]);
    v(2) = extract<double>(input[2]);
    return v;
  }

  Vector3d array(boost::python::tuple input) {
    Vector3d v;
    v(0) = extract<double>(input[0]);
    v(1) = extract<double>(input[1]);
    v(2) = extract<double>(input[2]);
    return v;
  }

  boost::python::list tolist(Vector3d v) {
    boost::python::list l;
    l.append(v(0));
    l.append(v(1));
    l.append(v(2));
    return l;
  }

  mat tomat(Vector3d v) {
    mat ret;
    vec ve;
    ve.push_back(v(0));
    ve.push_back(v(1));
    ve.push_back(v(2));
    ret.push_back(ve);
    return ret;
  }

  mat tomat(Matrix3d m) {
    mat ret;
    vec row1;
    vec row2;
    vec row3;

    row1.push_back(m(0,0));
    row1.push_back(m(0,1));
    row1.push_back(m(0,2));

    row2.push_back(m(1,0));
    row2.push_back(m(1,1));
    row2.push_back(m(1,2));

    row3.push_back(m(2,0));
    row3.push_back(m(2,1));
    row3.push_back(m(2,2));

    ret.push_back(row1);
    ret.push_back(row2);
    ret.push_back(row3);

    return ret;
  }

  boost::python::list tolist(Matrix3d m) {
    boost::python::list ret;
    boost::python::list row1;
    boost::python::list row2;
    boost::python::list row3;

    row1.append(m(0,0));
    row1.append(m(0,1));
    row1.append(m(0,2));

    row2.append(m(1,0));
    row2.append(m(1,1));
    row2.append(m(1,2));

    row3.append(m(2,0));
    row3.append(m(2,1));
    row3.append(m(2,2));

    ret.append(row1);
    ret.append(row2);
    ret.append(row3);

    return ret;
  }

  np::ndarray convert_to_numpy(mat const & input) {
    u_int n_rows = input.size();
    u_int n_cols = input[0].size();
    boost::python::tuple shape = boost::python::make_tuple(n_rows, n_cols);
    boost::python::tuple stride = boost::python::make_tuple(sizeof(double));
    np::dtype dtype = np::dtype::get_builtin<double>();
    boost::python::object own;
    np::ndarray converted = np::zeros(shape, dtype);

    for (u_int i = 0; i < n_rows; i++)
    {
        shape = boost::python::make_tuple(n_cols);
        converted[i] = np::from_data(input[i].data(), dtype, shape, stride, own);
    }
    return converted;
  }

};

BOOST_PYTHON_MODULE(Arm_ext) {
    class_<Arm>("Arm", init<boost::python::list, boost::python::list,
    boost::python::list, boost::python::tuple, char*>())
        .def("getFrames", &Arm::getFrames)
        .def_readwrite("numDOF", &Arm::numDOF)
        .def_readwrite("name", &Arm::name)
        .def_readwrite("velocity_limits", &Arm::velocity_limits)
        .def_readwrite("rotOffsets", &Arm::rotOffsets)
        .def_readwrite("dispOffset", &Arm::dispOffset)
        .def_readwrite("joint_limits", &Arm::joint_limits)
        .def_readwrite("displacements", &Arm::displacements)
        .def_readwrite("axes", &Arm::axes)
    ;
}