// This file is part of PoseEstimation.
// This file is a modified version of p3p.m <http://rpg.ifi.uzh.ch/software_datasets.html>,
// see 3-Clause BSD license below.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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

#ifndef P3P_HPP_
#define P3P_HPP_

#include <Eigen/Dense>

class P3PTest;

namespace P3P
{
template <typename FloatType>
class P3P {
	friend ::P3PTest;

private:
	typedef Eigen::Matrix<std::complex<FloatType>, 4, 1> CMatrix_4x1;
	typedef Eigen::Matrix<FloatType, 3, 1> Matrix_3x1;
	typedef Eigen::Matrix<FloatType, 3, 3> Matrix_3x3;
	typedef Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> Matrix_3x3R;
	typedef Eigen::Matrix<FloatType, 4, 1> Matrix_4x1;
	typedef Eigen::Matrix<FloatType, 4, 4> Matrix_4x4;
	const static FloatType m_eps;

public:
	// x = R'*(X - t) =>  x = R*X + t
	static void convertPose(FloatType *R, FloatType *t)
	{
		Eigen::Map<Matrix_3x3R> mR1(R);
		Eigen::Map<Matrix_3x1> mt1(t);
		Matrix_3x3 mR2 = mR1.transpose();
		Matrix_3x1 mt2 = -mR2*mt1;
		mR1 = mR2;
		mt1 = mt2;
	}

	// x = R'*(X - t)
	bool getMatrix(const FloatType *p2dn, const FloatType *p3d, int &nSols, std::vector<FloatType> &Ps)
	{
		// check degenerate case
		if (checkColinearity(p3d))
		{
			nSols = 0;
			Ps.resize(0);
			return false;
		}

		// select P1, f1 and compute T matrix, N matrix
		selectP1(p2dn, p3d);
		transformCoordinate();

		// compute constraints
		getConstraints();

		// compute real roots for the constraints
		getRealRoot();
		if (m_realRoots.empty())
		{
			nSols = 0;
			Ps.resize(0);
			return true;
		}

		// compute poses corresponding to the real root
		getPoses(nSols, Ps);

		return true;
	}

private:
	inline void getConstraints()
	{
		FloatType pcotn[2] = {m_p[1], m_p[0] * m_phi[0] / m_phi[1] - m_d12 * m_b}; // numerator of Eq. 9
		FloatType pcotd[2] = {m_p[1] * m_phi[0] / m_phi[1], -m_p[0] +  m_d12}; // denominator of Eq. 9
		FloatType pcotn2[3] = {}, pcotd2[3] = {}, pcotnd[3] = {}; // nume^2, denom^2, nume*denom
		polynomialMultiplication1(pcotn, pcotn, pcotn2);
		polynomialMultiplication1(pcotd, pcotd, pcotd2);
		polynomialMultiplication1(pcotn, pcotd, pcotnd);
		FloatType pcotd2n2[3] = {}; // denom^2 + nume^2
		pcotd2n2[0] = pcotd2[0] + pcotn2[0];
		pcotd2n2[1] = pcotd2[1] + pcotn2[1];
		pcotd2n2[2] = pcotd2[2] + pcotn2[2];

		FloatType pcos1[] = {-m_phi[1] * m_phi[1] * m_p[1] * m_p[1], 0, 0}; // term 1 of l.h.s of Eq.10, I think f_2^2 in original equation is phi_2^2
		pcos1[2] = -pcos1[0];
		polynomialMultiplication2(pcos1, pcotd2n2, m_as);

		FloatType p12 = m_p[0] * m_p[0];
		FloatType p122 = 2 * m_p[0] * m_p[1];
		FloatType p22 = m_p[1] * m_p[1];
		m_as[2] -= (p12 * pcotd2[0]); // term 1 of r.h.s of Eq.10
		m_as[3] -= (p12 * pcotd2[1]);
		m_as[4] -= (p12 * pcotd2[2]);
		m_as[1] += (p122 * pcotnd[0]); // term 2 of r.h.s of Eq.10
		m_as[2] += (p122 * pcotnd[1]);
		m_as[3] += (p122 * pcotnd[2]);
		m_as[0] -= (p22 * pcotn2[0]); // term 3 of r.h.s of Eq.10
		m_as[1] -= (p22 * pcotn2[1]);
		m_as[2] -= (p22 * pcotn2[2]);
	}

	void getPoses(int &nSols, std::vector<FloatType> &Ps)
	{
		nSols = 0;
		Ps.resize((3 + 9) * m_realRoots.size());
		for (size_t k = 0; k < m_realRoots.size(); k++)
		{
			nSols++;
			const FloatType cos_theta = m_realRoots[k];
			const FloatType sin_theta = std::sqrt(1 - (cos_theta * cos_theta));
			const FloatType cot_alpha =
				(m_phi[0] * m_p[0] / m_phi[1] + cos_theta * m_p[1] - m_d12 * m_b) /
				(m_phi[0] * m_p[1] * cos_theta / m_phi[1] - m_p[0] + m_d12); // Eq. 9
			const FloatType sin_alpha = std::sqrt(1 / (cot_alpha * cot_alpha + 1));
			FloatType cos_alpha = std::sqrt(1 - (sin_alpha * sin_alpha));
			if (cot_alpha < 0)
			{
				cos_alpha = -cos_alpha;
			}

			FloatType *pC = &(Ps[(3 + 9) * k]);
			Eigen::Map<Matrix_3x1> mC(pC); // Eq. 5
			mC[0] = cos_alpha;
			mC[1] = sin_alpha * cos_theta;
			mC[2] = sin_alpha * sin_theta;

			Eigen::Map<Matrix_3x3R> mR(pC + 3); // Eq. 6
			mR(0,0) = -mC[0];
			mR(0,1) = -mC[1];
			mR(0,2) = -mC[2];
			mR(1, 0) = sin_alpha;
			mR(1, 1) = -cos_alpha * cos_theta;
			mR(1, 2) = -cos_alpha * sin_theta;
			mR(2, 0) = 0;
			mR(2, 1) = -sin_theta;
			mR(2, 2) = cos_theta;

			mC *= (m_d12 * (sin_alpha * m_b + cos_alpha));
			mC = m_mP1 + m_mN.transpose() * mC; // Eq. 12
			mR = m_mN.transpose() * mR.transpose() * m_mT; // Eq. 13
		}
	}

	inline void getRealRoot()
	{
		m_realRoots.resize(0);
		Matrix_4x4 mCom; // companion matrix
		mCom.setZero();
		mCom(1, 0) = mCom(2, 1) = mCom(3, 2) = 1;
		mCom(0, 3) = -m_as[4] / m_as[0];
		mCom(1, 3) = -m_as[3] / m_as[0];
		mCom(2, 3) = -m_as[2] / m_as[0];
		mCom(3, 3) = -m_as[1] / m_as[0];
		Eigen::EigenSolver<Matrix_4x4> eig(mCom, false);
		CMatrix_4x1 mSols = eig.eigenvalues();

		for (int k = 0; k < 4; k++)
		{
			if (std::abs(mSols[k].imag()) < m_eps)
			{
				m_realRoots.push_back(mSols[k].real());
			}
		}
	}

	inline void selectP1(const FloatType *f, const FloatType *P)
	{
		const FloatType *f1 = f;
		const FloatType *f2 = f1 + 3;
		const FloatType *f3 = f2 + 3;
		// compute 3rd row of T
		FloatType T3[3] = {f1[1] * f2[2] - f1[2] * f2[1], f1[2] * f2[0] - f1[0] * f2[2], f1[0] * f2[1] - f1[1] * f2[0]};
		FloatType sign = T3[0] * f3[0] + T3[1] * f3[1] + T3[2] * f3[2];
		if (sign > 0)
		{
			m_f[0] = f + 3;
			m_f[1] = f;
			m_P[0] = P + 3;
			m_P[1] = P;
		}
		else
		{
			m_f[0] = f;
			m_f[1] = f + 3;
			m_P[0] = P;
			m_P[1] = P + 3;
		}
		m_f[2] = f + 6;
		m_P[2] = P + 6;
		m_mP1[0] = m_P[0][0];
		m_mP1[1] = m_P[0][1];
		m_mP1[2] = m_P[0][2];
	}

	void transformCoordinate()
	{
		// compute T matrix
		m_mT(0, 0) = m_f[0][0];
		m_mT(0, 1) = m_f[0][1];
		m_mT(0, 2) = m_f[0][2];
		m_mT(1, 0) = m_f[1][0];
		m_mT(1, 1) = m_f[1][1];
		m_mT(1, 2) = m_f[1][2];
		FloatType cos_beta = m_mT.row(0).dot(m_mT.row(1));
		m_mT.row(2) = m_mT.row(0).cross(m_mT.row(1));
		m_mT.row(1) = m_mT.row(2).cross(m_mT.row(0));
		m_mT.rowwise().normalize();

		// compute N matrix
		m_mN(0, 0) = m_P[1][0] - m_P[0][0];
		m_mN(0, 1) = m_P[1][1] - m_P[0][1];
		m_mN(0, 2) = m_P[1][2] - m_P[0][2];
		m_d12 = m_mN.row(0).norm();
		m_mN(1, 0) = m_P[2][0] - m_P[0][0];
		m_mN(1, 1) = m_P[2][1] - m_P[0][1];
		m_mN(1, 2) = m_P[2][2] - m_P[0][2];
		m_mN.row(2) = m_mN.row(0).cross(m_mN.row(1));
		m_mN.row(1) = m_mN.row(2).cross(m_mN.row(0));
		m_mN.rowwise().normalize();

		// compute b
		m_b = std::sqrt((1 / (1 - (cos_beta * cos_beta))) - 1); // Eq. 3
		if (cos_beta < 0)
		{
			m_b = -m_b;
		}

		// compute phis
		Matrix_3x1 tmp(m_f[2]);
		tmp = m_mT * tmp;
		m_phi[0] = tmp[0] / tmp[2]; // Eq. 8
		m_phi[1] = tmp[1] / tmp[2]; // Eq. 8

		// compute ps
		tmp[0] = m_P[2][0];
		tmp[1] = m_P[2][1];
		tmp[2] = m_P[2][2];
		tmp = m_mN * (tmp - m_mP1); // Eq. 2
		m_p[0] = tmp[0];
		m_p[1] = tmp[1];
	}

private:
	static inline bool checkColinearity(const FloatType *p)
	{
		FloatType p12[3] = {p[3] - p[0], p[4] - p[1], p[5] - p[2]};
		FloatType p13[3] = {p[6] - p[0], p[7] - p[1], p[8] - p[2]};
		FloatType pcross[] = {p12[2] * p13[1] - p12[1] * p13[2], p12[0] * p13[2] - p12[2] * p13[0], p12[1] * p13[0] - p12[0] * p13[1]};
		FloatType ncross = std::sqrt(pcross[0] * pcross[0] + pcross[1] * pcross[1] + pcross[2] * pcross[2]);
		return (ncross < m_eps);
	}

	// 1st order polynomial multiplication
	// (ax + b) * (cx + d) = (acx^2 + (ad + bc)x + bd)
	// p1 = [x 1]
	// p2 = [x 1]
	// p3 = [x^2 x 1]
	static inline void polynomialMultiplication1(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);					// x^2
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);	// x
		p3[2] = (p1[1])*(p2[1]);					// 1
	}

	// 2nd order polynomial multiplication
	// (ax^2 + bx + c) * (dx^2 + ex + f) = (adx^4 + (ae + bd)x^3 + (af + be + cd)x^2 + (bf + ce)x + cf
	// p1 = [x^2 x 1]
	// p2 = [x^2 x 1]
	// p3 = [x^4 x^3 x^2 x 1]
	static inline void polynomialMultiplication2(const FloatType *p1, const FloatType *p2, FloatType *p3)
	{
		p3[0] = (p1[0])*(p2[0]);										// x^4
		p3[1] = (p1[0])*(p2[1]) + (p1[1])*(p2[0]);						// x^3
		p3[2] = (p1[0])*(p2[2]) + (p1[1])*(p2[1]) + (p1[2])*(p2[0]);	// x^2
		p3[3] = (p1[1])*(p2[2]) + (p1[2])*(p2[1]);						// x
		p3[4] = (p1[2])*(p2[2]);										// 1
	}

private:
	const FloatType *m_f[3]; // pointer to 2D feature vector
	const FloatType *m_P[3]; // pointer to 3D feature vector
	Matrix_3x1 m_mP1;
	Matrix_3x3 m_mT;
	Matrix_3x3 m_mN;
	FloatType m_phi[2];
	FloatType m_p[2];
	FloatType m_b;
	FloatType m_d12;
	FloatType m_as[5]; // coefficient of 4th order polynomial
	std::vector<FloatType> m_realRoots; // real root for cos_theta polynomial
};

template <typename FloatType>
const FloatType P3P<FloatType>::m_eps = 1E-7;
}

#endif // P3P_HPP_


// Boost.Geometry

// Copyright (c) 2018 Yaghyavardhan Singh Khangarot, Hyderabad, India.
// Contributed and/or modified by Yaghyavardhan Singh Khangarot,
//   as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP

#include <algorithm>

#ifdef BOOST_GEOMETRY_DEBUG_FRECHET_DISTANCE
#include <iostream>
#endif

#include <iterator>
#include <utility>
#include <vector>
#include <limits>

#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/distance_result.hpp>
#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace discrete_frechet_distance
{

template <typename size_type1 , typename size_type2,typename result_type>
class coup_mat
{
public:
    coup_mat(size_type1 w, size_type2 h)
        : m_data(w * h,-1), m_width(w), m_height(h)
    {}

    result_type & operator()(size_type1 i, size_type2 j)
    {
        BOOST_GEOMETRY_ASSERT(i < m_width && j < m_height);
        return m_data[j * m_width + i];
    }

private:
    std::vector<result_type> m_data;
    size_type1 m_width;
    size_type2 m_height;
};

struct linestring_linestring
{
    template <typename Linestring1, typename Linestring2, typename Strategy>
    static inline typename distance_result
        <
            typename point_type<Linestring1>::type,
            typename point_type<Linestring2>::type,
            Strategy
        >::type apply(Linestring1 const& ls1, Linestring2 const& ls2, Strategy const& strategy)
    {
        typedef typename distance_result
            <
                typename point_type<Linestring1>::type,
                typename point_type<Linestring2>::type,
                Strategy
            >::type result_type;
        typedef typename boost::range_size<Linestring1>::type size_type1;
        typedef typename boost::range_size<Linestring2>::type size_type2;

        boost::geometry::detail::throw_on_empty_input(ls1);
        boost::geometry::detail::throw_on_empty_input(ls2);

        size_type1 const a = boost::size(ls1);
        size_type2 const b = boost::size(ls2);

        //Coupling Matrix CoupMat(a,b,-1);
        coup_mat<size_type1,size_type2,result_type> coup_matrix(a,b);

        result_type const not_feasible = -100;
        //findin the Coupling Measure
        for (size_type1 i = 0 ; i < a ; i++ )
        {
            for(size_type2 j=0;j<b;j++)
            {
                result_type dis = strategy.apply(range::at(ls1,i), range::at(ls2,j));
                if(i==0 && j==0)
                    coup_matrix(i,j) = dis;
                else if(i==0 && j>0)
                    coup_matrix(i,j) =
                        (std::max)(coup_matrix(i,j-1), dis);
                else if(i>0 && j==0)
                    coup_matrix(i,j) =
                        (std::max)(coup_matrix(i-1,j), dis);
                else if(i>0 && j>0)
                    coup_matrix(i,j) =
                        (std::max)((std::min)(coup_matrix(i,j-1),
                                              (std::min)(coup_matrix(i-1,j),
                                                         coup_matrix(i-1,j-1))),
                                   dis);
                else
                    coup_matrix(i,j) = not_feasible;
            }
        }

        #ifdef BOOST_GEOMETRY_DEBUG_FRECHET_DISTANCE
        //Print CoupLing Matrix
        for(size_type i = 0; i <a; i++)
        {
            for(size_type j = 0; j <b; j++)
            std::cout << coup_matrix(i,j) << " ";
            std::cout << std::endl;
        }
        #endif

        return coup_matrix(a-1,b-1);
    }
};

}} // namespace detail::frechet_distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{
template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type
>
struct discrete_frechet_distance : not_implemented<Tag1, Tag2>
{};

template <typename Linestring1, typename Linestring2>
struct discrete_frechet_distance
    <
        Linestring1,
        Linestring2,
        linestring_tag,
        linestring_tag
    >
    : detail::discrete_frechet_distance::linestring_linestring
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief Calculate discrete Frechet distance between two geometries (currently
       works for LineString-LineString) using specified strategy.
\ingroup discrete_frechet_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy A type fulfilling a DistanceStrategy concept
\param geometry1 Input geometry
\param geometry2 Input geometry
\param strategy Distance strategy to be used to calculate Pt-Pt distance

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/discrete_frechet_distance.qbk]}

\qbk{
[heading Available Strategies]
\* [link geometry.reference.strategies.strategy_distance_pythagoras Pythagoras (cartesian)]
\* [link geometry.reference.strategies.strategy_distance_haversine Haversine (spherical)]
[/ \* more (currently extensions): Vincenty\, Andoyer (geographic) ]

[heading Example]
[discrete_frechet_distance_strategy]
[discrete_frechet_distance_strategy_output]
}
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline typename distance_result
        <
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type,
            Strategy
        >::type
discrete_frechet_distance(Geometry1 const& geometry1,
                          Geometry2 const& geometry2,
                          Strategy const& strategy)
{
    return dispatch::discrete_frechet_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, strategy);
}

// Algorithm overload using default Pt-Pt distance strategy

/*!
\brief Calculate discrete Frechet distance between two geometries (currently
       work for LineString-LineString).
\ingroup discrete_frechet_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 Input geometry
\param geometry2 Input geometry

\qbk{[include reference/algorithms/discrete_frechet_distance.qbk]}

\qbk{
[heading Example]
[discrete_frechet_distance]
[discrete_frechet_distance_output]
}
*/
template <typename Geometry1, typename Geometry2>
inline typename distance_result
        <
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type
discrete_frechet_distance(Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    typedef typename strategy::distance::services::default_strategy
              <
                  point_tag, point_tag,
                  typename point_type<Geometry1>::type,
                  typename point_type<Geometry2>::type
              >::type strategy_type;

    return discrete_frechet_distance(geometry1, geometry2, strategy_type());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP


/*
 ==============================================================================
 KratosStructuralApplication
 A library based on:
 Kratos
 A General Purpose Software for Multi-Physics Finite Element Analysis
 Version 1.0 (Released on march 05, 2007).

 Copyright 2007
 Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
 pooyan@cimne.upc.edu
 rrossi@cimne.upc.edu
 janosch.stascheit@rub.de
 nagel@sd.rub.de
 - CIMNE (International Center for Numerical Methods in Engineering),
 Gran Capita' s/n, 08034 Barcelona, Spain
 - Ruhr-University Bochum, Institute for Structural Mechanics, Germany

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

 Distribution of this code for  any  commercial purpose  is permissible
 ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

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

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

 ==============================================================================
 */
/* *********************************************************
 *
 *   Last Modified by:    $Author: hbui $
 *   Date:                $Date: 2013 Dec 15 $
 *   Revision:            $Revision: 1.1 $
 *
 * ***********************************************************/

// System includes
// External includes
#include <boost/timer.hpp>
// Project includes
#include "includes/define.h"
#include "custom_elements/kinematic_linear_nurbs.h"
#include "utilities/math_utils.h"
#include "utilities/openmp_utils.h"
#include "structural_application/custom_utilities/sd_math_utils.h"
#include "isogeometric_application/isogeometric_application.h"

//#define ENABLE_DEBUG_CONSTITUTIVE_LAW

//#define DEBUG_LEVEL1
//#define DEBUG_LEVEL2

#define ENUMERATE_DOF_LIST_COUPLE
//#define ENUMERATE_DOF_LIST_SEQUENTIAL //just for debugging and compare with geopde

//#define CHECK_JACOBIAN

#define ENABLE_PROFILING

namespace Kratos
{

KinematicLinearNURBS::KinematicLinearNURBS(IndexType NewId,
        GeometryType::Pointer pGeometry) :
        KinematicLinear2(NewId, pGeometry)
{
    KRATOS_WATCH("At KinematicLinearNURBS Default Constructor")
    mIsInitialized = false;
    mpIsogeometricGeometry =
        boost::dynamic_pointer_cast<IsogeometricGeometryType>(pGetGeometry());
}

/**
 * A simple kinematic linear 3D element for the solution
 * of the momentum balance in structural mechanics.
 * This element is used for students training at the Ruhr University Bochum.
 * Therefore it may includes comments that are obvious for the
 * experienced user.
 * hbui modified to work in 2D
 */
KinematicLinearNURBS::KinematicLinearNURBS(IndexType NewId,
        GeometryType::Pointer pGeometry, PropertiesType::Pointer pProperties) :
        KinematicLinear2(NewId, pGeometry, pProperties)
{
    KRATOS_WATCH("At KinematicLinearNURBS Constructor")
    mIsInitialized = false;
    mpIsogeometricGeometry =
        boost::dynamic_pointer_cast<IsogeometricGeometryType>(pGetGeometry());
}

Element::Pointer KinematicLinearNURBS::Create(IndexType NewId,
        NodesArrayType const& ThisNodes,
        PropertiesType::Pointer pProperties) const
{
    KRATOS_WATCH("At KinematicLinearNURBS::Create")
    GeometryType::Pointer pGeometry = GetGeometry().Create(ThisNodes);
    KRATOS_WATCH("At KinematicLinearNURBS::Create created geometry")
    KRATOS_WATCH(*pProperties)
    Element::Pointer pElement = Element::Pointer(
            new KinematicLinearNURBS(NewId, pGeometry, pProperties));
    KRATOS_WATCH("KinematicLinearNURBS::Create completed")
    return pElement;
}

Element::Pointer KinematicLinearNURBS::Create(IndexType NewId,
        NodesArrayType const& ThisNodes) const
{
    return Element::Pointer(
            new KinematicLinearNURBS(NewId,
                    GetGeometry().Create(ThisNodes)));
}

KinematicLinearNURBS::~KinematicLinearNURBS()
{
}

/**
 * Initialization of the element, called at the begin of each simulation.
 * Membervariables and the Material law are initialized here
 */
void KinematicLinearNURBS::Initialize(const ProcessInfo& rCurrentProcessInfo)
{
    KRATOS_TRY		//EXCEPTION HANDLING (see corresponing KRATOS_CATCH("") )

        KRATOS_WATCH("At Initialize")

        if (mIsInitialized)
        {
            //dimension of the problem
            unsigned int dim = GetGeometry().WorkingSpaceDimension();

            //Set Up Initial displacement for StressFreeActivation of Elements
            mInitialDisp.resize(GetGeometry().size(), dim, false);

            for (unsigned int node = 0; node < GetGeometry().size(); node++)
                for (unsigned int i = 0; i < dim; i++) // hbui edited
                    mInitialDisp(node, i) =
                            GetGeometry()[node].GetSolutionStepValue(
                                    DISPLACEMENT)[i];

            return;
        }

        ///////////////////////////////////////////////////////////////
        // One time initialisation
        ///////////////////////////////////////////////////////////////

        ////////////////////Initialize geometry_data/////////////////////////////
        #ifdef ENABLE_PROFILING
        double start_compute, end_compute;
        start_compute = OpenMPUtils::GetCurrentTime();
        #endif

        //initialize the nurbs geometry
        // try to read the extraction operator from the elemental data
        Matrix ExtractionOperator;
        if( this->Has( EXTRACTION_OPERATOR ) )
        {
            ExtractionOperator = this->GetValue( EXTRACTION_OPERATOR );
        }
        else if( this->Has( EXTRACTION_OPERATOR_MCSR ) )
        {
            Matrix Temp = this->GetValue( EXTRACTION_OPERATOR_MCSR );

            // make a simple check
            if(Temp.size1() != 2)
                KRATOS_THROW_ERROR(std::logic_error, "Invalid MCSR matrix for extraction operator found at element", this->Id())

            // choose the best storage scheme based ratio between number of nonzeros and the full size of the matrix
            unsigned int size_ex_n = (unsigned int)(Temp(0, 0) - 1);
            unsigned int size_ex_nz = Temp.size2() - 1;
            if( ( (double)(size_ex_nz) ) / (size_ex_n * size_ex_n) < 0.2 )
                ExtractionOperator = IsogeometricMathUtils::MCSR2CSR(Temp);
            else
                ExtractionOperator = IsogeometricMathUtils::MCSR2MAT(Temp);
        }
        else if( this->Has( EXTRACTION_OPERATOR_CSR_ROWPTR )
             && this->Has( EXTRACTION_OPERATOR_CSR_COLIND )
             && this->Has( EXTRACTION_OPERATOR_CSR_VALUES ) )
        {
            Vector rowPtr = this->GetValue( EXTRACTION_OPERATOR_CSR_ROWPTR ); // must be 0-base
            Vector colInd = this->GetValue( EXTRACTION_OPERATOR_CSR_COLIND ); // must be 0-base
            Vector values = this->GetValue( EXTRACTION_OPERATOR_CSR_VALUES );
            ExtractionOperator = IsogeometricMathUtils::Triplet2CSR(rowPtr, colInd, values);
        }

//        KRATOS_WATCH(ExtractionOperator)

        mpIsogeometricGeometry->AssignGeometryData(
            this->GetValue(NURBS_KNOTS_1),
            this->GetValue(NURBS_KNOTS_2),
            this->GetValue(NURBS_KNOTS_3),
            this->GetValue(NURBS_WEIGHTS),
            ExtractionOperator,
            this->GetValue(NURBS_DEGREE_1),
            this->GetValue(NURBS_DEGREE_2),
            this->GetValue(NURBS_DEGREE_3),
            2 // only need to compute 2 integration rules
        );

        mThisIntegrationMethod = mpIsogeometricGeometry->GetDefaultIntegrationMethod(); //default method

        InitializeJacobian();

        #ifdef ENABLE_PROFILING
        end_compute = OpenMPUtils::GetCurrentTime();
        std::cout << "GenerateGeometryData for element " << Id() << " completed: " << end_compute - start_compute << " s" << std::endl;
        #endif
        ////////////////////End Initialize geometry_data/////////////////////////////

        unsigned int dim = GetGeometry().WorkingSpaceDimension();
        //Set Up Initial displacement for StressFreeActivation of Elements
        mInitialDisp.resize(GetGeometry().size(), dim, false);

        for (unsigned int node = 0; node < GetGeometry().size(); node++)
            for (unsigned int i = 0; i < dim; ++i)
                mInitialDisp(node, i) =
                        GetGeometry()[node].GetSolutionStepValue(DISPLACEMENT)[i];

        //Initialization of the constitutive law vector and
        // declaration, definition and initialization of the material
        // law was at each integration point
        const GeometryType::IntegrationPointsArrayType& integration_points =
                GetGeometry().IntegrationPoints(mThisIntegrationMethod);
        if (mConstitutiveLawVector.size() != integration_points.size())
        {
            mConstitutiveLawVector.resize(integration_points.size());
        }

        InitializeMaterial();

        mIsInitialized = true;

    KRATOS_CATCH( "" )
}

void KinematicLinearNURBS::InitializeJacobian1D()
{
    //TODO
}

void KinematicLinearNURBS::InitializeJacobian()
{
    unsigned int dim = GetGeometry().WorkingSpaceDimension();

    //number of integration points used, mThisIntegrationMethod refers to the
    //integration method defined in the constructor
    const GeometryType::IntegrationPointsArrayType& integration_points =
            GetGeometry().IntegrationPoints(mThisIntegrationMethod);

    //initializing the Jacobian, the inverse Jacobian and Jacobians determinant in the reference
    // configuration
    GeometryType::JacobiansType J0(integration_points.size());

    mInvJ0.resize(integration_points.size());

    mTotalDomainInitialSize = 0.00;

    for (unsigned int i = 0; i < integration_points.size(); ++i)
    {
        mInvJ0[i].resize(dim, dim, false);
        noalias(mInvJ0[i]) = ZeroMatrix(dim, dim);
    }

    mDetJ0.resize(integration_points.size(), false);

    noalias(mDetJ0) = ZeroVector(integration_points.size());

    //calculating the Jacobian
    J0 = GetGeometry().Jacobian(J0, mThisIntegrationMethod);

    //calculating the inverse Jacobian
    #pragma omp parallel for
    for (unsigned int PointNumber = 0; PointNumber < integration_points.size(); ++PointNumber)
    {
        //getting informations for integration
        double IntegrationWeight = integration_points[PointNumber].Weight();
        //calculating and storing inverse of the jacobian and the parameters needed
        MathUtils<double>::InvertMatrix(J0[PointNumber], mInvJ0[PointNumber], mDetJ0[PointNumber]);
        //calculating the total area
        #pragma omp atomic
        mTotalDomainInitialSize += mDetJ0[PointNumber] * IntegrationWeight;
    }
}

int KinematicLinearNURBS::Check(const Kratos::ProcessInfo& rCurrentProcessInfo)
{
    KRATOS_TRY

        unsigned int dimension = this->GetGeometry().WorkingSpaceDimension();

        if (this->Id() < 1)
        {
            KRATOS_THROW_ERROR(std::logic_error,
                    "Element found with Id 0 or negative", "");
        }

        if (this->GetGeometry().Length() < 0)
        {
            std::cout << "error on element -> " << this->Id() << std::endl;
            KRATOS_THROW_ERROR(std::logic_error, "Length can not be less than 0. Please check Jacobian.", __FUNCTION__);
        }

        if (this->GetGeometry().Area() < 0)
        {
            std::cout << "error on element -> " << this->Id() << std::endl;
            KRATOS_THROW_ERROR(std::logic_error, "Area can not be less than 0. Please check Jacobian.", __FUNCTION__);
        }

        if (this->GetGeometry().Volume() < 0)
        {
            std::cout << "error on element -> " << this->Id() << std::endl;
            KRATOS_THROW_ERROR(std::logic_error, "Volume can not be less than 0. Please check Jacobian.", __FUNCTION__);
        }

        //verify that the constitutive law exists
        if (this->GetProperties().Has(CONSTITUTIVE_LAW) == false)
        {
            KRATOS_THROW_ERROR(std::logic_error,
                    "constitutive law not provided for property ",
                    this->GetProperties().Id());
        }

        //Verify that the body force is defined
        if (this->GetProperties().Has(BODY_FORCE) == false)
        {
            KRATOS_THROW_ERROR(std::logic_error,
                    "BODY_FORCE not provided for property ",
                    this->GetProperties().Id())
        }

        //verify that the constitutive law has the correct dimension
        if (dimension == 2)
        {
            if (this->GetProperties().Has(THICKNESS) == false)
                KRATOS_THROW_ERROR(std::logic_error,
                        "THICKNESS not provided for element ", this->Id());

            if (this->GetProperties().GetValue(CONSTITUTIVE_LAW)->GetStrainSize() != 3)
                KRATOS_THROW_ERROR(std::logic_error,
                        "wrong constitutive law used. This is a 2D element! expected strain size is 3 (el id = ) ",
                        this->Id());
        }
        else if (dimension == 3)
        {
            if (this->GetProperties().GetValue(CONSTITUTIVE_LAW)->GetStrainSize() != 6)
                KRATOS_THROW_ERROR(std::logic_error,
                        "wrong constitutive law used. This is a 3D element! expected strain size is 6 (el id = ) ",
                        this->Id());
        }

        //check constitutive law
        int ok = 0;
        for (unsigned int i = 0; i < mConstitutiveLawVector.size(); i++)
        {
            ok = mConstitutiveLawVector[i]->Check(GetProperties(),
                    GetGeometry(), rCurrentProcessInfo);
            if (ok != 0)
            {
                KRATOS_THROW_ERROR(std::logic_error, "Something wrong with the consitutive law", __FUNCTION__)
            }

//			if( mConstitutiveLawVector[i]->IsIncremental() )
//				KRATOS_THROW_ERROR( std::logic_error, "This element does not provide incremental strains!", "" );
//			if( mConstitutiveLawVector[i]->GetStrainMeasure() != ConstitutiveLaw::StrainMeasure_Linear )
//				KRATOS_THROW_ERROR( std::logic_error, "This element formulated in linear strain measure", "" );
//			if( mConstitutiveLawVector[i]->GetStressMeasure() != ConstitutiveLaw::StressMeasure_PK1 )
//				KRATOS_THROW_ERROR( std::logic_error, "This element is formulated in PK1 stresses", "" );
        }

        //check Jacobian
        #ifdef CHECK_JACOBIAN
        GeometryType::CoordinatesArrayType P;

        P[0] = 0.0;
        P[1] = 0.0;
        P[2] = 0.0;

        double J0 = GetGeometry().DeterminantOfJacobian( P );

        if(J0 < 0.0)
        {
            KRATOS_THROW_ERROR(std::logic_error, "Negative Jacobian is detected", __FUNCTION__)
        }
        #endif

        return ok;

    KRATOS_CATCH( "" );

}

} // Namespace Kratos

#undef ENABLE_DEBUG_CONSTITUTIVE_LAW
#undef DEBUG_LEVEL1
#undef DEBUG_LEVEL2
#undef ENUMERATE_DOF_LIST_SEQUENTIAL
#undef ENUMERATE_DOF_LIST_COUPLE
#undef CHECK_JACOBIAN
#undef ENABLE_PROFILING


#include "Graph.h"

#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

using namespace graph;

BOOST_AUTO_TEST_SUITE(GraphIsomorphismTests)

BOOST_AUTO_TEST_CASE(WhenConstructed_VertexesCount)
{
  Graph g(2);
  Graph h(4);

  BOOST_CHECK(g.Size() == 2);
  BOOST_CHECK(h.Size() == 4);
}

BOOST_AUTO_TEST_CASE(WhenConstructed_ThenEdgesCountIsZero)
{
  Graph g(3);

  BOOST_CHECK(g.GetEdgesCount() == 0);
}

BOOST_AUTO_TEST_CASE(WhenEdgeInserted_ThenEdgesCountIncremented)
{
  Graph g(3);

  g.AddEdge(1, 2);

  BOOST_CHECK(g.GetEdgesCount() == 1);
}

BOOST_AUTO_TEST_CASE(WhenSeveralEdgesInserted_ThenEdgesCountIsValid)
{
  Graph g(3);

  g.AddEdge(0, 1);
  g.AddEdge(1, 2);
  g.AddEdge(2, 0);

  BOOST_CHECK(g.GetEdgesCount() == 3);
}

BOOST_AUTO_TEST_CASE(WhenInvalidEdge_ThenReturnFalse)
{
  Graph g(3);

  BOOST_CHECK(!g.AddEdge(2, 3));
}

BOOST_AUTO_TEST_CASE(WhenDoubledEdge_ThenReturnFalse)
{
  Graph g(3);
  g.AddEdge(2, 3);

  BOOST_CHECK(!g.AddEdge(2, 3));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism1_WhenSizeNotEqual)
{
  Graph g(4);
  Graph h(3);

  std::vector<size_t> dummy;
  BOOST_CHECK(!g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism_WhenEdgesCountNotEqual)
{
  Graph g(4);
  g.AddEdge(0, 1);
  g.AddEdge(1, 2);
  g.AddEdge(2, 3);

  Graph h(4);
  h.AddEdge(1, 2);
  h.AddEdge(2, 3);

  std::vector<size_t> dummy;
  BOOST_CHECK(!g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism1)
{
  Graph g(4);
  Graph h(4);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism2)
{
  Graph g(4);
  g.AddEdge(0, 1);
  g.AddEdge(1, 2);
  g.AddEdge(2, 3);

  Graph h(4);
  h.AddEdge(1, 2);
  h.AddEdge(2, 3);
  h.AddEdge(3, 0);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism3)
{
  Graph g(4);
  g.AddEdge(0, 1);
  g.AddEdge(1, 2);
  g.AddEdge(2, 3);
  g.AddEdge(3, 0);
  g.AddEdge(3, 1);

  Graph h(4);
  h.AddEdge(1, 3);
  h.AddEdge(3, 2);
  h.AddEdge(2, 0);
  h.AddEdge(0, 1);
  h.AddEdge(1, 2);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism4)
{
  Graph g(7);
  g.AddEdge(0, 2);
  g.AddEdge(0, 6);
  g.AddEdge(0, 5);
  g.AddEdge(1, 6);
  g.AddEdge(1, 4);
  g.AddEdge(2, 3);
  g.AddEdge(3, 4);
  g.AddEdge(5, 6);

  Graph h(7);
  h.AddEdge(0, 1);
  h.AddEdge(0, 5);
  h.AddEdge(1, 3);
  h.AddEdge(2, 3);
  h.AddEdge(2, 6);
  h.AddEdge(4, 2);
  h.AddEdge(4, 6);
  h.AddEdge(5, 6);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism5)
{
  Graph g(3);
  g.AddEdge(0, 0);
  g.AddEdge(1, 1);
  g.AddEdge(2, 2);

  Graph h(3);
  h.AddEdge(1, 1);
  h.AddEdge(0, 0);
  h.AddEdge(2, 2);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.GetEdgesCount() == 3);
  BOOST_CHECK(g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism6)
{
  Graph g(3);
  g.AddEdge(0, 0);
  g.AddEdge(1, 1);
  g.AddEdge(2, 2);
  g.AddEdge(1, 2);

  Graph h(3);
  h.AddEdge(1, 1);
  h.AddEdge(0, 0);
  h.AddEdge(2, 2);
  h.AddEdge(2, 0);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.GetEdgesCount() == 4);
  BOOST_CHECK(g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_CASE(CheckIsomorphism7)
{
  Graph g(3);
  g.AddEdge(0, 0);
  g.AddEdge(1, 1);
  g.AddEdge(2, 2);

  Graph h(3);
  h.AddEdge(1, 1);
  h.AddEdge(0, 0);
  h.AddEdge(2, 2);
  h.AddEdge(2, 0);

  std::vector<size_t> dummy;
  BOOST_CHECK(g.GetEdgesCount() == 3);
  BOOST_CHECK(h.GetEdgesCount() == 4);
  BOOST_CHECK(!g.AreIsomorphic(h, dummy));
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file decision_tree_test.cpp
 * @author Ryan Curtin
 *
 * Tests for the DecisionTree class and related classes.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/decision_tree/decision_tree.hpp>
#include <mlpack/methods/decision_tree/information_gain.hpp>
#include <mlpack/methods/decision_tree/gini_gain.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::distribution;

/**
 * Create a mock categorical dataset for testing.
 */
void MockCategoricalData(arma::mat& d,
                         arma::Row<size_t>& l,
                         data::DatasetInfo& datasetInfo)
{
  // We'll build a spiral dataset plus two noisy categorical features.  We need
  // to build the distributions for the categorical features (they'll be
  // discrete distributions).
  DiscreteDistribution c1[5];
  // The distribution will be automatically normalized.
  for (size_t i = 0; i < 5; ++i)
  {
    std::vector<arma::vec> probs;
    probs.push_back(arma::vec(4, arma::fill::randu));
    c1[i] = DiscreteDistribution(probs);
  }

  DiscreteDistribution c2[5];
  for (size_t i = 0; i < 5; ++i)
  {
    std::vector<arma::vec> probs;
    probs.push_back(arma::vec(2, arma::fill::randu));
    c2[i] = DiscreteDistribution(probs);
  }

  arma::mat spiralDataset(4, 4000);
  arma::Row<size_t> labels(4000);
  for (size_t i = 0; i < 4000; ++i)
  {
    // One circle every 2000 samples.  Plus some noise.
    const double magnitude = 2.0 + (double(i) / 200.0) +
        0.5 * mlpack::math::Random();
    const double angle = (i % 200) * (2 * M_PI) + mlpack::math::Random();

    const double x = magnitude * cos(angle);
    const double y = magnitude * sin(angle);

    spiralDataset(0, i) = x;
    spiralDataset(1, i) = y;

    // Set categorical features c1 and c2.
    if (i < 800)
    {
      spiralDataset(2, i) = c1[1].Random()[0];
      spiralDataset(3, i) = c2[1].Random()[0];
      labels[i] = 1;
    }
    else if (i < 1600)
    {
      spiralDataset(2, i) = c1[3].Random()[0];
      spiralDataset(3, i) = c2[3].Random()[0];
      labels[i] = 3;
    }
    else if (i < 2400)
    {
      spiralDataset(2, i) = c1[2].Random()[0];
      spiralDataset(3, i) = c2[2].Random()[0];
      labels[i] = 2;
    }
    else if (i < 3200)
    {
      spiralDataset(2, i) = c1[0].Random()[0];
      spiralDataset(3, i) = c2[0].Random()[0];
      labels[i] = 0;
    }
    else
    {
      spiralDataset(2, i) = c1[4].Random()[0];
      spiralDataset(3, i) = c2[4].Random()[0];
      labels[i] = 4;
    }
  }

  // Now create the dataset info.
  datasetInfo = data::DatasetInfo(4);
  datasetInfo.Type(2) = data::Datatype::categorical;
  datasetInfo.Type(3) = data::Datatype::categorical;
  // Set mappings.
  datasetInfo.MapString<double>("0", 2);
  datasetInfo.MapString<double>("1", 2);
  datasetInfo.MapString<double>("2", 2);
  datasetInfo.MapString<double>("3", 2);
  datasetInfo.MapString<double>("0", 3);
  datasetInfo.MapString<double>("1", 3);

  // Now shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0, 3999,
      4000));
  d = arma::mat(4, 4000);
  l = arma::Row<size_t>(4000);
  for (size_t i = 0; i < 4000; ++i)
  {
    d.col(i) = spiralDataset.col(indices[i]);
    l[i] = labels[indices[i]];
  }
}

BOOST_AUTO_TEST_SUITE(DecisionTreeTest);

/**
 * Make sure the Gini gain is zero when the labels are perfect.
 */
BOOST_AUTO_TEST_CASE(GiniGainPerfectTest)
{
  arma::rowvec weights(10, arma::fill::ones);
  arma::Row<size_t> labels;
  labels.zeros(10);

  // Test that it's perfect regardless of number of classes.
  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(GiniGain::Evaluate<false>(labels, c, weights), 1e-5);
}

/**
 * Make sure the Gini gain is -0.5 when the class split between two classes
 * is even.
 */
BOOST_AUTO_TEST_CASE(GiniGainEvenSplitTest)
{
  arma::rowvec weights = arma::ones<arma::rowvec>(10);
  arma::Row<size_t> labels(10);
  for (size_t i = 0; i < 5; ++i)
    labels[i] = 0;
  for (size_t i = 5; i < 10; ++i)
    labels[i] = 1;

  // Test that it's -0.5 regardless of the number of classes.
  for (size_t c = 2; c < 10; ++c)
  {
    BOOST_REQUIRE_CLOSE(
        GiniGain::Evaluate<false>(labels, c, weights), -0.5, 1e-5);
    double weightedGain = GiniGain::Evaluate<true>(labels, c, weights);

    // The weighted gain should stay the same with unweight one
    BOOST_REQUIRE_EQUAL(
        GiniGain::Evaluate<false>(labels, c, weights), weightedGain);
  }
}

/**
 * The Gini gain of an empty vector is 0.
 */
BOOST_AUTO_TEST_CASE(GiniGainEmptyTest)
{
  arma::rowvec weights = arma::ones<arma::rowvec>(10);
  // Test across some numbers of classes.
  arma::Row<size_t> labels;
  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(GiniGain::Evaluate<false>(labels, c, weights), 1e-5);

  for (size_t c = 1; c < 10; ++c)
    BOOST_REQUIRE_SMALL(GiniGain::Evaluate<true>(labels, c, weights), 1e-5);
}

/**
 * The Gini gain is -(1 - 1/k) for k classes evenly split.
 */
BOOST_AUTO_TEST_CASE(GiniGainEvenSplitManyClassTest)
{
  // Try with many different classes.
  for (size_t c = 2; c < 30; ++c)
  {
    arma::Row<size_t> labels(c);
    arma::rowvec weights(c);
    for (size_t i = 0; i < c; ++i)
    {
      labels[i] = i;
      weights[i] = 1;
    }

    // Calculate Gini gain and make sure it is correct.
    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate<false>(labels, c, weights),
        -(1.0 - 1.0 / c), 1e-5);
    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate<true>(labels, c, weights),
        -(1.0 - 1.0 / c), 1e-5);
  }
}

/**
 * The Gini gain should not be sensitive to the number of points.
 */
BOOST_AUTO_TEST_CASE(GiniGainManyPoints)
{
  for (size_t i = 1; i < 20; ++i)
  {
    const size_t numPoints = 100 * i;
    arma::rowvec weights(numPoints);
    weights.ones();
    arma::Row<size_t> labels(numPoints);
    for (size_t j = 0; j < numPoints / 2; ++j)
      labels[j] = 0;
    for (size_t j = numPoints / 2; j < numPoints; ++j)
      labels[j] = 1;

    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate<false>(labels, 2, weights), -0.5,
        1e-5);
    BOOST_REQUIRE_CLOSE(GiniGain::Evaluate<true>(labels, 2, weights), -0.5,
        1e-5);
  }
}

/**
 * To make sure the Gini gain can been cacluate proporately with weight.
 */
BOOST_AUTO_TEST_CASE(GiniGainWithWeight)
{
  arma::Row<size_t> labels(10);
  arma::rowvec weights(10);
  for (size_t i = 0; i < 5; ++i)
  {
    labels[i] = 0;
    weights[i] = 0.3;
  }
  for (size_t i = 5; i < 10; ++i)
  {
    labels[i] = 1;
    weights[i] = 0.7;
  }

  BOOST_REQUIRE_CLOSE(
      GiniGain::Evaluate<true>(labels, 2, weights), -0.42, 1e-5);
}

/**
 * The information gain should be zero when the labels are perfect.
 */
BOOST_AUTO_TEST_CASE(InformationGainPerfectTest)
{
  arma::rowvec weights;
  arma::Row<size_t> labels;
  labels.zeros(10);

  // Test that it's perfect regardless of number of classes.
  for (size_t c = 1; c < 10; ++c)
  {
    BOOST_REQUIRE_SMALL(
        InformationGain::Evaluate<false>(labels, c, weights), 1e-5);
  }
}

/**
 * If we have an even split, the information gain should be -1.
 */
BOOST_AUTO_TEST_CASE(InformationGainEvenSplitTest)
{
  arma::Row<size_t> labels(10);
  arma::rowvec weights(10);
  weights.ones();
  for (size_t i = 0; i < 5; ++i)
    labels[i] = 0;
  for (size_t i = 5; i < 10; ++i)
    labels[i] = 1;

  // Test that it's -1 regardless of the number of classes.
  for (size_t c = 2; c < 10; ++c)
  {
    // Weighted and unweighted result should be the same.
    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate<false>(labels, c, weights),
        -1.0, 1e-5);
    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate<true>(labels, c, weights),
        -1.0, 1e-5);
  }
}

/**
 * The information gain of an empty vector is 0.
 */
BOOST_AUTO_TEST_CASE(InformationGainEmptyTest)
{
  arma::Row<size_t> labels;
  arma::rowvec weights = arma::ones<arma::rowvec>(10);
  for (size_t c = 1; c < 10; ++c)
  {
    BOOST_REQUIRE_SMALL(InformationGain::Evaluate<false>(labels, c, weights),
        1e-5);
    BOOST_REQUIRE_SMALL(InformationGain::Evaluate<true>(labels, c, weights),
        1e-5);
  }
}

/**
 * The information gain is log2(1/k) when splitting equal classes.
 */
BOOST_AUTO_TEST_CASE(InformationGainEvenSplitManyClassTest)
{
  arma::rowvec weights;
  // Try with many different numbers of classes.
  for (size_t c = 2; c < 30; ++c)
  {
    arma::Row<size_t> labels(c);
    for (size_t i = 0; i < c; ++i)
      labels[i] = i;

    // Calculate information gain and make sure it is correct.
    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate<false>(labels, c, weights),
        std::log2(1.0 / c), 1e-5);
  }
}

/**
 * Test the information gain with weighted labels
 */
BOOST_AUTO_TEST_CASE(InformationWithWeight)
{
  arma::Row<size_t> labels(10);
  arma::rowvec weights("1 1 1 1 1 0 0 0 0 0");
  for (size_t i = 0; i < 5; ++i)
    labels[i] = 0;
  for (size_t i = 5; i < 10; ++i)
    labels[i] = 1;

  // Zero is not a good result as gain, but we just need to prove
  // cacluation works.
  BOOST_REQUIRE_CLOSE(
      InformationGain::Evaluate<true>(labels, 2, weights), 0, 1e-5);
}

/**
 * The information gain should not be sensitive to the number of points.
 */
BOOST_AUTO_TEST_CASE(InformationGainManyPoints)
{
  for (size_t i = 1; i < 20; ++i)
  {
    const size_t numPoints = 100 * i;
    arma::Row<size_t> labels(numPoints);
    arma::rowvec weights = arma::ones<arma::rowvec>(numPoints);
    for (size_t j = 0; j < numPoints / 2; ++j)
      labels[j] = 0;
    for (size_t j = numPoints / 2; j < numPoints; ++j)
      labels[j] = 1;

    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate<false>(labels, 2, weights),
        -1.0, 1e-5);
    // It should make no difference between a weighted and unweighted
    // calculation.
    BOOST_REQUIRE_CLOSE(InformationGain::Evaluate<true>(labels, 2, weights),
        -1.0, 1e-5);
  }
}

/**
 * Check that the BestBinaryNumericSplit will split on an obviously splittable
 * dimension.
 */
BOOST_AUTO_TEST_CASE(BestBinaryNumericSplitSimpleSplitTest)
{
  arma::vec values("0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0");
  arma::Row<size_t> labels("0 0 0 0 0 1 1 1 1 1 1");
  arma::rowvec weights(labels.n_elem);
  weights.ones();

  arma::vec classProbabilities;
  BestBinaryNumericSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate<false>(labels, 2, weights);
  const double gain = BestBinaryNumericSplit<GiniGain>::SplitIfBetter<false>(
      bestGain, values, labels, 2, weights, 3, classProbabilities, aux);
  const double weightedGain =
      BestBinaryNumericSplit<GiniGain>::SplitIfBetter<true>(bestGain, values,
      labels, 2, weights, 3, classProbabilities, aux);

  // Make sure that a split was made.
  BOOST_REQUIRE_GT(gain, bestGain);

  // Make sure weight works and make no different with no weighted one
  BOOST_REQUIRE_EQUAL(gain, weightedGain);

  // The split is perfect, so we should be able to accomplish a gain of 0.
  BOOST_REQUIRE_SMALL(gain, 1e-5);

  // The class probabilities, for this split, hold the splitting point, which
  // should be between 4 and 5.
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 1);
  BOOST_REQUIRE_GT(classProbabilities[0], 0.4);
  BOOST_REQUIRE_LT(classProbabilities[0], 0.5);
}

/**
 * Check that the BestBinaryNumericSplit won't split if not enough points are
 * given.
 */
BOOST_AUTO_TEST_CASE(BestBinaryNumericSplitMinSamplesTest)
{
  arma::vec values("0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0");
  arma::Row<size_t> labels("0 0 0 0 0 1 1 1 1 1 1");
  arma::rowvec weights(labels.n_elem);

  arma::vec classProbabilities;
  BestBinaryNumericSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate<false>(labels, 2, weights);
  const double gain = BestBinaryNumericSplit<GiniGain>::SplitIfBetter<false>(
      bestGain, values, labels, 2, weights, 8, classProbabilities, aux);
  // This should make no difference because it won't split at all.
  const double weightedGain =
      BestBinaryNumericSplit<GiniGain>::SplitIfBetter<true>(bestGain, values,
      labels, 2, weights, 8, classProbabilities, aux);

  // Make sure that no split was made.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(gain, weightedGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * Check that the BestBinaryNumericSplit doesn't split a dimension that gives no
 * gain.
 */
BOOST_AUTO_TEST_CASE(BestBinaryNumericSplitNoGainTest)
{
  arma::vec values(100);
  arma::Row<size_t> labels(100);
  arma::rowvec weights;
  for (size_t i = 0; i < 100; i += 2)
  {
    values[i] = i;
    labels[i] = 0;
    values[i + 1] = i;
    labels[i + 1] = 1;
  }

  arma::vec classProbabilities;
  BestBinaryNumericSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate<false>(labels, 2, weights);
  const double gain = BestBinaryNumericSplit<GiniGain>::SplitIfBetter<false>(
      bestGain, values, labels, 2, weights, 10, classProbabilities, aux);

  // Make sure there was no split.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * Check that the AllCategoricalSplit will split when the split is obviously
 * better.
 */
BOOST_AUTO_TEST_CASE(AllCategoricalSplitSimpleSplitTest)
{
  arma::vec values("0 0 0 1 1 1 2 2 2 3 3 3");
  arma::Row<size_t> labels("0 0 0 2 2 2 1 1 1 2 2 2");
  arma::rowvec weights(labels.n_elem);
  weights.ones();

  arma::vec classProbabilities;
  AllCategoricalSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate<false>(labels, 3, weights);
  const double gain = AllCategoricalSplit<GiniGain>::SplitIfBetter<false>(
      bestGain, values, 4, labels, 3, weights, 3, classProbabilities, aux);
  const double weightedGain =
      AllCategoricalSplit<GiniGain>::SplitIfBetter<true>(bestGain, values, 4,
      labels, 3, weights, 3, classProbabilities, aux);

  // Make sure that a split was made.
  BOOST_REQUIRE_GT(gain, bestGain);

  // Since the split is perfect, make sure the new gain is 0.
  BOOST_REQUIRE_SMALL(gain, 1e-5);

  BOOST_REQUIRE_EQUAL(gain, weightedGain);

  // Make sure the class probabilities now hold the number of children.
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 1);
  BOOST_REQUIRE_EQUAL((size_t) classProbabilities[0], 4);
}

/**
 * Make sure that AllCategoricalSplit respects the minimum number of samples
 * required to split.
 */
BOOST_AUTO_TEST_CASE(AllCategoricalSplitMinSamplesTest)
{
  arma::vec values("0 0 0 1 1 1 2 2 2 3 3 3");
  arma::Row<size_t> labels("0 0 0 2 2 2 1 1 1 2 2 2");
  arma::rowvec weights(labels.n_elem);
  weights.ones();

  arma::vec classProbabilities;
  AllCategoricalSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate<false>(labels, 3, weights);
  const double gain = AllCategoricalSplit<GiniGain>::SplitIfBetter<false>(
      bestGain, values, 4, labels, 3, weights, 4, classProbabilities, aux);

  // Make sure it's not split.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * Check that no split is made when it doesn't get us anything.
 */
BOOST_AUTO_TEST_CASE(AllCategoricalSplitNoGainTest)
{
  arma::vec values(300);
  arma::Row<size_t> labels(300);
  arma::rowvec weights = arma::ones<arma::rowvec>(300);

  for (size_t i = 0; i < 300; i += 3)
  {
    values[i] = (i / 3) % 10;
    labels[i] = 0;
    values[i + 1] = (i / 3) % 10;
    labels[i + 1] = 1;
    values[i + 2] = (i / 3) % 10;
    labels[i + 2] = 2;
  }

  arma::vec classProbabilities;
  AllCategoricalSplit<GiniGain>::template AuxiliarySplitInfo<double> aux;

  // Call the method to do the splitting.
  const double bestGain = GiniGain::Evaluate<false>(labels, 3, weights);
  const double gain = AllCategoricalSplit<GiniGain>::SplitIfBetter<false>(
      bestGain, values, 10, labels, 3, weights, 10, classProbabilities, aux);
  const double weightedGain =
      AllCategoricalSplit<GiniGain>::SplitIfBetter<true>(bestGain, values, 10,
      labels, 3, weights, 10, classProbabilities, aux);

  // Make sure that there was no split.
  BOOST_REQUIRE_EQUAL(gain, bestGain);
  BOOST_REQUIRE_EQUAL(gain, weightedGain);
  BOOST_REQUIRE_EQUAL(classProbabilities.n_elem, 0);
}

/**
 * A basic construction of the decision tree---ensure that we can create the
 * tree and that it split at least once.
 */
BOOST_AUTO_TEST_CASE(BasicConstructionTest)
{
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);

  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.

  // Use default parameters.
  DecisionTree<> d(dataset, labels, 3, 50);

  // Now require that we have some children.
  BOOST_REQUIRE_GT(d.NumChildren(), 0);
}

/**
 * Construct a tree with weighted labels.
 */
BOOST_AUTO_TEST_CASE(BasicConstructionTestWithWeight)
{
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  arma::rowvec weights(labels.n_elem);
  weights.ones();

  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.

  // Use default parameters.
  DecisionTree<> wd(dataset, labels, 3, weights, 50);
  DecisionTree<> d(dataset, labels, 3, 50);

  // Now require that we have some children.
  BOOST_REQUIRE_GT(wd.NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(wd.NumChildren(), d.NumChildren());
}

/**
 * Construct the decision tree on numeric data only and see that we can fit it
 * exactly and achieve perfect performance on the training set.
 */
BOOST_AUTO_TEST_CASE(PerfectTrainingSet)
{
  // Completely random dataset with no structure.
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.
  arma::rowvec weights(labels.n_elem);
  weights.ones();

  DecisionTree<> d(dataset, labels, 3, 1); // Minimum leaf size of 1.

  // Make sure that we can get perfect accuracy on the training set.
  for (size_t i = 0; i < 1000; ++i)
  {
    size_t prediction;
    arma::vec probabilities;
    d.Classify(dataset.col(i), prediction, probabilities);

    BOOST_REQUIRE_EQUAL(prediction, labels[i]);
    BOOST_REQUIRE_EQUAL(probabilities.n_elem, 3);
    for (size_t j = 0; j < 3; ++j)
    {
      if (labels[i] == j)
        BOOST_REQUIRE_CLOSE(probabilities[j], 1.0, 1e-5);
      else
        BOOST_REQUIRE_SMALL(probabilities[j], 1e-5);
    }
  }
}

/**
 * onstruct the decision tree with weighted labels
 */
BOOST_AUTO_TEST_CASE(PerfectTrainingSetWithWeight)
{
  // Completely random dataset with no structure.
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.
  arma::rowvec weights(labels.n_elem);
  weights.ones();

  DecisionTree<> d(dataset, labels, 3, weights, 1); // Minimum leaf size of 1.

  // This part of code is dupliacte with no weighted one.
  for (size_t i = 0; i < 1000; ++i)
  {
    size_t prediction;
    arma::vec probabilities;
    d.Classify(dataset.col(i), prediction, probabilities);

    BOOST_REQUIRE_EQUAL(prediction, labels[i]);
    BOOST_REQUIRE_EQUAL(probabilities.n_elem, 3);
    for (size_t j = 0; j < 3; ++j)
    {
      if (labels[i] == j)
        BOOST_REQUIRE_CLOSE(probabilities[j], 1.0, 1e-5);
      else
        BOOST_REQUIRE_SMALL(probabilities[j], 1e-5);
    }
  }
}

/**
 * Make sure class probabilities are computed correctly in the root node.
 */
BOOST_AUTO_TEST_CASE(ClassProbabilityTest)
{
  arma::mat dataset(5, 100, arma::fill::randu);
  arma::Row<size_t> labels(100);
  for (size_t i = 0; i < 100; i += 2)
  {
    labels[i] = 0;
    labels[i + 1] = 1;
  }

  // Create a decision tree that can't split.
  DecisionTree<> d(dataset, labels, 2, 1000);

  BOOST_REQUIRE_EQUAL(d.NumChildren(), 0);

  // Estimate a point's probabilities.
  arma::vec probabilities;
  size_t prediction;
  d.Classify(dataset.col(0), prediction, probabilities);

  BOOST_REQUIRE_EQUAL(probabilities.n_elem, 2);
  BOOST_REQUIRE_CLOSE(probabilities[0], 0.5, 1e-5);
  BOOST_REQUIRE_CLOSE(probabilities[1], 0.5, 1e-5);
}

/**
 * Test that the decision tree generalizes reasonably.
 */
BOOST_AUTO_TEST_CASE(SimpleGeneralizationTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.csv!");

  arma::Row<size_t> labels;
  if (!data::Load("vc2_labels.txt", labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // Initialize an all-ones weight matrix.
  arma::rowvec weights(labels.n_cols, arma::fill::ones);

  // Build decision tree.
  DecisionTree<> d(inputData, labels, 3, 10); // Leaf size of 10.
  DecisionTree<> wd(inputData, labels, 3, weights, 10); // Leaf size of 10.

  // Load testing data.
  arma::mat testData;
  if (!data::Load("vc2_test.csv", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.csv!");

  arma::Mat<size_t> trueTestLabels;
  if (!data::Load("vc2_test_labels.txt", trueTestLabels))
    BOOST_FAIL("Cannot load labels for vc2_test_labels.txt");

  // Get the predicted test labels.
  arma::Row<size_t> predictions;
  d.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);

  // Figure out the accuracy.
  double correct = 0.0;
  for (size_t i = 0; i < predictions.n_elem; ++i)
    if (predictions[i] == trueTestLabels[i])
      ++correct;
  correct /= predictions.n_elem;

  BOOST_REQUIRE_GT(correct, 0.75);

  // reset the prediction
  predictions.zeros();
  wd.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);

  // Figure out the accuracy.
  double wdcorrect = 0.0;
  for (size_t i = 0; i < predictions.n_elem; ++i)
    if (predictions[i] == trueTestLabels[i])
      ++wdcorrect;
  wdcorrect /= predictions.n_elem;

  BOOST_REQUIRE_GT(wdcorrect, 0.75);
}

/**
 * Test that we can build a decision tree on a simple categorical dataset.
 */
BOOST_AUTO_TEST_CASE(CategoricalBuildTest)
{
  arma::mat d;
  arma::Row<size_t> l;
  data::DatasetInfo di;
  MockCategoricalData(d, l, di);

  // Split into a training set and a test set.
  arma::mat trainingData = d.cols(0, 1999);
  arma::mat testData = d.cols(2000, 3999);
  arma::Row<size_t> trainingLabels = l.subvec(0, 1999);
  arma::Row<size_t> testLabels = l.subvec(2000, 3999);

  // Build the tree.
  DecisionTree<> tree(trainingData, di, trainingLabels, 5, 10);

  // Now evaluate the accuracy of the tree.
  arma::Row<size_t> predictions;
  tree.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);
  size_t correct = 0;
  for (size_t i = 0; i < testData.n_cols; ++i)
    if (testLabels[i] == predictions[i])
      ++correct;

  // Make sure we got at least 70% accuracy.
  const double correctPct = double(correct) / double(testData.n_cols);
  BOOST_REQUIRE_GT(correctPct, 0.70);
}

/**
 * Test that we can build a decision tree with weights on a simple categorical
 * dataset.
 */
BOOST_AUTO_TEST_CASE(CategoricalBuildTestWithWeight)
{
  arma::mat d;
  arma::Row<size_t> l;
  data::DatasetInfo di;
  MockCategoricalData(d, l, di);

  // Split into a training set and a test set.
  arma::mat trainingData = d.cols(0, 1999);
  arma::mat testData = d.cols(2000, 3999);
  arma::Row<size_t> trainingLabels = l.subvec(0, 1999);
  arma::Row<size_t> testLabels = l.subvec(2000, 3999);

  arma::Row<double> weights = arma::ones<arma::Row<double>>(
      trainingLabels.n_elem);

  // Build the tree.
  DecisionTree<> tree(trainingData, di, trainingLabels, 5, weights, 10);

  // Now evaluate the accuracy of the tree.
  arma::Row<size_t> predictions;
  tree.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);
  size_t correct = 0;
  for (size_t i = 0; i < testData.n_cols; ++i)
    if (testLabels[i] == predictions[i])
      ++correct;

  // Make sure we got at least 70% accuracy.
  const double correctPct = double(correct) / double(testData.n_cols);
  BOOST_REQUIRE_GT(correctPct, 0.70);
}

/**
 * Make sure that when we ask for a decision stump, we get one.
 */
BOOST_AUTO_TEST_CASE(DecisionStumpTest)
{
  // Use a random dataset.
  arma::mat dataset(10, 1000, arma::fill::randu);
  arma::Row<size_t> labels(1000);
  for (size_t i = 0; i < 1000; ++i)
    labels[i] = i % 3; // 3 classes.

  // Build a decision stump.
  DecisionTree<GiniGain, BestBinaryNumericSplit, AllCategoricalSplit,
      AllDimensionSelect, double, true> stump(dataset, labels, 3, 1);

  // Check that it has children.
  BOOST_REQUIRE_EQUAL(stump.NumChildren(), 2);
  // Check that its children doesn't have children.
  BOOST_REQUIRE_EQUAL(stump.Child(0).NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(stump.Child(1).NumChildren(), 0);
}

/**
 * Test that we can build a decision tree using weighted data (where the
 * low-weighted data is random noise), and that the tree still builds correctly
 * enough to get good results.
 */
BOOST_AUTO_TEST_CASE(WeightedDecisionTreeTest)
{
  arma::mat dataset;
  arma::Row<size_t> labels;
  data::Load("vc2.csv", dataset);
  data::Load("vc2_labels.txt", labels);

  // Add some noise.
  arma::mat noise(dataset.n_rows, 1000, arma::fill::randu);
  arma::Row<size_t> noiseLabels(1000);
  for (size_t i = 0; i < noiseLabels.n_elem; ++i)
    noiseLabels[i] = math::RandInt(3); // Random label.

  // Concatenate data matrices.
  arma::mat data = arma::join_rows(dataset, noise);
  arma::Row<size_t> fullLabels = arma::join_rows(labels, noiseLabels);

  // Now set weights.
  arma::rowvec weights(dataset.n_cols + 1000);
  for (size_t i = 0; i < dataset.n_cols; ++i)
    weights[i] = math::Random(0.9, 1.0);
  for (size_t i = dataset.n_cols; i < dataset.n_cols + 1000; ++i)
    weights[i] = math::Random(0.0, 0.01); // Low weights for false points.

  // Now build the decision tree.  I think the syntax is right here.
  DecisionTree<> d(data, fullLabels, 3, weights, 10);

  // Now we can check that we get good performance on the VC2 test set.
  arma::mat testData;
  arma::Row<size_t> testLabels;
  data::Load("vc2_test.csv", testData);
  data::Load("vc2_test_labels.txt", testLabels);

  arma::Row<size_t> predictions;
  d.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);

  // Figure out the accuracy.
  double correct = 0.0;
  for (size_t i = 0; i < predictions.n_elem; ++i)
    if (predictions[i] == testLabels[i])
      ++correct;
  correct /= predictions.n_elem;

  BOOST_REQUIRE_GT(correct, 0.75);
}
/**
 * Test that we can build a decision tree on a simple categorical dataset using
 * weights, with low-weight noise added.
 */
BOOST_AUTO_TEST_CASE(CategoricalWeightedBuildTest)
{
  arma::mat d;
  arma::Row<size_t> l;
  data::DatasetInfo di;
  MockCategoricalData(d, l, di);

  // Split into a training set and a test set.
  arma::mat trainingData = d.cols(0, 1999);
  arma::mat testData = d.cols(2000, 3999);
  arma::Row<size_t> trainingLabels = l.subvec(0, 1999);
  arma::Row<size_t> testLabels = l.subvec(2000, 3999);

  // Now create random points.
  arma::mat randomNoise(4, 2000);
  arma::Row<size_t> randomLabels(2000);
  for (size_t i = 0; i < 2000; ++i)
  {
    randomNoise(0, i) = math::Random();
    randomNoise(1, i) = math::Random();
    randomNoise(2, i) = math::RandInt(4);
    randomNoise(3, i) = math::RandInt(2);
    randomLabels[i] = math::RandInt(5);
  }

  // Generate weights.
  arma::rowvec weights(4000);
  for (size_t i = 0; i < 2000; ++i)
    weights[i] = math::Random(0.9, 1.0);
  for (size_t i = 2000; i < 4000; ++i)
    weights[i] = math::Random(0.0, 0.001);

  arma::mat fullData = arma::join_rows(trainingData, randomNoise);
  arma::Row<size_t> fullLabels = arma::join_rows(trainingLabels, randomLabels);

  // Build the tree.
  DecisionTree<> tree(fullData, di, fullLabels, 5, weights, 10);

  // Now evaluate the accuracy of the tree.
  arma::Row<size_t> predictions;
  tree.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);
  size_t correct = 0;
  for (size_t i = 0; i < testData.n_cols; ++i)
    if (testLabels[i] == predictions[i])
      ++correct;

  // Make sure we got at least 70% accuracy.
  const double correctPct = double(correct) / double(testData.n_cols);
  BOOST_REQUIRE_GT(correctPct, 0.70);
}

/**
 * Test that we can build a decision tree using weighted data (where the
 * low-weighted data is random noise) with information gain, and that the tree
 * still builds correctly enough to get good results.
 */
BOOST_AUTO_TEST_CASE(WeightedDecisionTreeInformationGainTest)
{
  arma::mat dataset;
  arma::Row<size_t> labels;
  data::Load("vc2.csv", dataset);
  data::Load("vc2_labels.txt", labels);

  // Add some noise.
  arma::mat noise(dataset.n_rows, 1000, arma::fill::randu);
  arma::Row<size_t> noiseLabels(1000);
  for (size_t i = 0; i < noiseLabels.n_elem; ++i)
    noiseLabels[i] = math::RandInt(3); // Random label.

  // Concatenate data matrices.
  arma::mat data = arma::join_rows(dataset, noise);
  arma::Row<size_t> fullLabels = arma::join_rows(labels, noiseLabels);

  // Now set weights.
  arma::rowvec weights(dataset.n_cols + 1000);
  for (size_t i = 0; i < dataset.n_cols; ++i)
    weights[i] = math::Random(0.9, 1.0);
  for (size_t i = dataset.n_cols; i < dataset.n_cols + 1000; ++i)
    weights[i] = math::Random(0.0, 0.01); // Low weights for false points.

  // Now build the decision tree.  I think the syntax is right here.
  DecisionTree<InformationGain> d(data, fullLabels, 3, weights, 10);

  // Now we can check that we get good performance on the VC2 test set.
  arma::mat testData;
  arma::Row<size_t> testLabels;
  data::Load("vc2_test.csv", testData);
  data::Load("vc2_test_labels.txt", testLabels);

  arma::Row<size_t> predictions;
  d.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);

  // Figure out the accuracy.
  double correct = 0.0;
  for (size_t i = 0; i < predictions.n_elem; ++i)
    if (predictions[i] == testLabels[i])
      ++correct;
  correct /= predictions.n_elem;

  BOOST_REQUIRE_GT(correct, 0.75);
}
/**
 * Test that we can build a decision tree using information gain on a simple
 * categorical dataset using weights, with low-weight noise added.
 */
BOOST_AUTO_TEST_CASE(CategoricalInformationGainWeightedBuildTest)
{
  arma::mat d;
  arma::Row<size_t> l;
  data::DatasetInfo di;
  MockCategoricalData(d, l, di);

  // Split into a training set and a test set.
  arma::mat trainingData = d.cols(0, 1999);
  arma::mat testData = d.cols(2000, 3999);
  arma::Row<size_t> trainingLabels = l.subvec(0, 1999);
  arma::Row<size_t> testLabels = l.subvec(2000, 3999);

  // Now create random points.
  arma::mat randomNoise(4, 2000);
  arma::Row<size_t> randomLabels(2000);
  for (size_t i = 0; i < 2000; ++i)
  {
    randomNoise(0, i) = math::Random();
    randomNoise(1, i) = math::Random();
    randomNoise(2, i) = math::RandInt(4);
    randomNoise(3, i) = math::RandInt(2);
    randomLabels[i] = math::RandInt(5);
  }

  // Generate weights.
  arma::rowvec weights(4000);
  for (size_t i = 0; i < 2000; ++i)
    weights[i] = math::Random(0.9, 1.0);
  for (size_t i = 2000; i < 4000; ++i)
    weights[i] = math::Random(0.0, 0.001);

  arma::mat fullData = arma::join_rows(trainingData, randomNoise);
  arma::Row<size_t> fullLabels = arma::join_rows(trainingLabels, randomLabels);

  // Build the tree.
  DecisionTree<InformationGain> tree(fullData, di, fullLabels, 5, weights, 10);

  // Now evaluate the accuracy of the tree.
  arma::Row<size_t> predictions;
  tree.Classify(testData, predictions);

  BOOST_REQUIRE_EQUAL(predictions.n_elem, testData.n_cols);
  size_t correct = 0;
  for (size_t i = 0; i < testData.n_cols; ++i)
    if (testLabels[i] == predictions[i])
      ++correct;

  // Make sure we got at least 70% accuracy.
  const double correctPct = double(correct) / double(testData.n_cols);
  BOOST_REQUIRE_GT(correctPct, 0.70);
}

/*
 * Test that we can pass const data into DecisionTree constructors.
 */
BOOST_AUTO_TEST_CASE(ConstDataTest)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);

  const arma::mat& constData = data;
  const arma::Row<size_t>& constLabels = labels;
  const arma::rowvec constWeigths(labels.n_elem, arma::fill::randu);
  const size_t numClasses = 5;

  DecisionTree<> dt(constData, constLabels, numClasses);
  DecisionTree<> dt2(constData, datasetInfo, constLabels, numClasses);
  DecisionTree<> dt3(constData, constLabels, numClasses, constWeigths);
  DecisionTree<> dt4(constData, datasetInfo, constLabels, numClasses,
      constWeigths);
}

BOOST_AUTO_TEST_SUITE_END();


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

#include <RBDyn/Jacobian.h>

#include "Tree30Dof.h"

#define BOOST_TEST_MODULE Expand
#include <RBDyn/FK.h>
#include <RBDyn/FV.h>

#include <boost/test/unit_test.hpp>

void setRandomFreeFlyer(rbd::MultiBodyConfig & mbc)
{
  Eigen::Vector3d axis = Eigen::Vector3d::Random();
  Eigen::AngleAxisd aa(0.5, axis / axis.norm());
  Eigen::Quaterniond qd(aa);
  mbc.q[0][0] = qd.w();
  mbc.q[0][1] = qd.x();
  mbc.q[0][2] = qd.y();
  mbc.q[0][3] = qd.z();
}

BOOST_AUTO_TEST_CASE(ExpandJacobianTest)
{
  std::srand(133757348);

  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;

  std::tie(mb, mbc, mbg) = makeTree30Dof(false);

  const static int ROUNDS = 1000;

  rbd::Jacobian jac(mb, mb.body(mb.nrBodies() - 1).name());
  rbd::Blocks compact = jac.compactPath(mb);

  for(int i = 0; i < ROUNDS; ++i)
  {
    mbc.zero(mb);

    Eigen::VectorXd q = Eigen::VectorXd::Random(mb.nrParams());
    mbc.q = rbd::vectorToParam(mb, q);
    setRandomFreeFlyer(mbc);

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

    Eigen::MatrixXd jacMat = jac.jacobian(mb, mbc);

    Eigen::MatrixXd fullJacMat(6, mb.nrDof());
    jac.fullJacobian(mb, jacMat, fullJacMat);

    Eigen::MatrixXd res = fullJacMat.transpose() * fullJacMat;

    Eigen::MatrixXd product = jacMat.transpose() * jacMat;
    Eigen::MatrixXd fullProduct = jac.expand(mb, product);

    BOOST_CHECK_EQUAL((fullProduct - res).norm(), 0);

    fullProduct.setZero();
    jac.expandAdd(mb, product, fullProduct);

    BOOST_CHECK_EQUAL((fullProduct - res).norm(), 0);

    fullProduct.setZero();
    jac.expandAdd(compact, product, fullProduct);

    BOOST_CHECK_EQUAL((fullProduct - res).norm(), 0);
  }
}


#include <boost/multiprecision/mpfi.hpp>


#include <common_robotics_utilities/serialization.hpp>

#include <cstdint>
#include <utility>
#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace serialization
{
constexpr uint64_t SerializedSizeVector2d()
{
  return (uint64_t)(sizeof(double) * 2);
}

constexpr uint64_t SerializedSizeVector3d()
{
  return (uint64_t)(sizeof(double) * 3);
}

constexpr uint64_t SerializedSizeVector4d()
{
  return (uint64_t)(sizeof(double) * 4);
}

constexpr uint64_t SerializedSizeQuaterniond()
{
  return (uint64_t)(sizeof(double) * 4);
}

constexpr uint64_t SerializedSizeIsometry3d()
{
  return (uint64_t)(sizeof(double) * 16);
}

uint64_t SerializeVectorXd(const Eigen::VectorXd& value,
                           std::vector<uint8_t>& buffer)
{
  return SerializeMemcpyableVectorLike<double, Eigen::VectorXd>(value, buffer);
}

uint64_t SerializeVector2d(const Eigen::Vector2d& value,
                           std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeVector2d(), 0x00);
  memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector2d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeVector2d();
}

uint64_t SerializeVector3d(const Eigen::Vector3d& value,
                           std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeVector3d(), 0x00);
  memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector3d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeVector3d();
}

uint64_t SerializeVector4d(const Eigen::Vector4d& value,
                           std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeVector4d(), 0x00);
  memcpy(&temp_buffer.front(), value.data(), SerializedSizeVector4d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeVector4d();
}

uint64_t SerializeQuaterniond(const Eigen::Quaterniond& value,
                              std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeQuaterniond(), 0x00);
  memcpy(&temp_buffer.front(),
         value.coeffs().data(),
         SerializedSizeQuaterniond());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeQuaterniond();
}

uint64_t SerializeIsometry3d(const Eigen::Isometry3d& value,
                             std::vector<uint8_t>& buffer)
{
  // Takes a state to serialize and a buffer to serialize into
  // Return number of bytes written to buffer
  std::vector<uint8_t> temp_buffer(SerializedSizeIsometry3d(), 0x00);
  memcpy(&temp_buffer.front(),
         value.matrix().data(),
         SerializedSizeIsometry3d());
  buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
  return SerializedSizeIsometry3d();
}

std::pair<Eigen::VectorXd, uint64_t> DeserializeVectorXd(
    const std::vector<uint8_t>& buffer, const uint64_t current)
{
  return DeserializeMemcpyableVectorLike<double, Eigen::VectorXd>(
      buffer, current);
}

std::pair<Eigen::Vector2d, uint64_t> DeserializeVector2d(
    const std::vector<uint8_t>& buffer, const uint64_t current)
{
  if (current >= buffer.size())
  {
    throw std::invalid_argument("current is outside the provided buffer");
  }
  if ((current + SerializedSizeVector2d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Vector2d temp_value;
  memcpy(temp_value.data(), &buffer[current], SerializedSizeVector2d());
  return std::make_pair(temp_value, SerializedSizeVector2d());
}

std::pair<Eigen::Vector3d, uint64_t> DeserializeVector3d(
    const std::vector<uint8_t>& buffer, const uint64_t current)
{
  if (current >= buffer.size())
  {
    throw std::invalid_argument("current is outside the provided buffer");
  }
  if ((current + SerializedSizeVector3d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Vector3d temp_value;
  memcpy(temp_value.data(), &buffer[current], SerializedSizeVector3d());
  return std::make_pair(temp_value, SerializedSizeVector3d());
}

std::pair<Eigen::Vector4d, uint64_t> DeserializeVector4d(
    const std::vector<uint8_t>& buffer, const uint64_t current)
{
  if (current >= buffer.size())
  {
    throw std::invalid_argument("current is outside the provided buffer");
  }
  if ((current + SerializedSizeVector4d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Vector4d temp_value;
  memcpy(temp_value.data(), &buffer[current], SerializedSizeVector4d());
  return std::make_pair(temp_value, SerializedSizeVector4d());
}

std::pair<Eigen::Quaterniond, uint64_t> DeserializeQuaterniond(
    const std::vector<uint8_t>& buffer, const uint64_t current)
{
  if (current >= buffer.size())
  {
    throw std::invalid_argument("current is outside the provided buffer");
  }
  if ((current + SerializedSizeQuaterniond()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Quaterniond temp_value;
  memcpy(temp_value.coeffs().data(),
         &buffer[current],
         SerializedSizeQuaterniond());
  return std::make_pair(temp_value, SerializedSizeQuaterniond());
}

std::pair<Eigen::Isometry3d, uint64_t> DeserializeIsometry3d(
    const std::vector<uint8_t>& buffer, const uint64_t current)
{
  if (current >= buffer.size())
  {
    throw std::invalid_argument("current is outside the provided buffer");
  }
  if ((current + SerializedSizeIsometry3d()) > buffer.size())
  {
    throw std::invalid_argument("Not enough room in the provided buffer");
  }
  // Takes a buffer to read from and the starting index in the buffer
  // Return the loaded state and how many bytes we read from the buffer
  Eigen::Isometry3d temp_value;
  memcpy(temp_value.matrix().data(),
         &buffer[current],
         SerializedSizeIsometry3d());
  return std::make_pair(temp_value, SerializedSizeIsometry3d());
}
}  // namespace serialization
}  // namespace common_robotics_utilities


#include <omp.h>
#include <cmath>
#include <algorithm>
#include <boost/foreach.hpp>
#include "node.hpp"

double Node::Strength() const {
  double weight_sum = 0.0;
  for( std::vector<Edge>::const_iterator it = m_edges.begin(); it != m_edges.end(); ++it) {
    weight_sum += it->weight;
  }
  return weight_sum;
}

Edge* Node::EdgeSelection(Node* parent_node) {
  double prob_sum = 0.0;
  std::vector<double> probs( m_edges.size(), 0.0 );
  for(CEdgeIt it = m_edges.begin(); it != m_edges.end(); ++it) {
    if( it->node != parent_node ) {
      prob_sum += it->weight;
    }
    probs[ it - m_edges.begin() ] = prob_sum;
  }

  double r = prob_sum * Random::Rand01( omp_get_thread_num() );
  std::vector<double>::iterator found = std::upper_bound(probs.begin(), probs.end(), r);
  assert( found != probs.end() );
  return &(m_edges[ found - probs.begin() ]);
}

Edge* Node::FindEdge(Node* nj) {
  for( EdgeIt it = m_edges.begin(); it != m_edges.end(); ++it) {
    if( it->node == nj ) { return &(*it); }
  }
  return NULL;
}

void Node::AddEdge(Node* nj, double initial_weight) {
  assert( FindEdge(nj) == NULL );
  m_edges.push_back(Edge(nj, initial_weight));
}

void Node::EnhanceEdge(Node* nj, double delta) {
  Edge* edge = FindEdge(nj);
  assert(edge != NULL);
  #pragma omp atomic
  edge->weight += delta;
}

void Node::DeleteEdge(Node* nj) {
  Edge* edge = FindEdge(nj);
  assert(edge != NULL);
  *edge = m_edges.back();
  m_edges.pop_back();
  assert( FindEdge(nj) == NULL );
}

class IsLessThanThreshold {
public:
  IsLessThanThreshold(double th) : m_threshold(th) {};
  bool operator()(Edge edge) const {
    return (edge.weight < m_threshold) ? true : false;
  }
private:
  const double m_threshold;

};

void Node::AgingEdge(double aging_factor, double threshold) {
  for( EdgeIt it = m_edges.begin(); it != m_edges.end(); ++it) {
    it->weight *= aging_factor;
  }
  IsLessThanThreshold pred(threshold);
  m_edges.erase( std::remove_if(m_edges.begin(), m_edges.end(), pred),
                 m_edges.end());
}


//----------------------------------------------------------------------------
/// @file test_spinsort.cpp
/// @brief test program of the spinsort algorithm
///
/// @author Copyright (c) 2016 Francisco José Tapia (fjtapia@gmail.com )\n
///         Distributed under the Boost Software License, Version 1.0.\n
///         ( See accompanying file LICENSE_1_0.txt or copy at
///           http://www.boost.org/LICENSE_1_0.txt  )
/// @version 0.1
///
/// @remarks
//-----------------------------------------------------------------------------
#include <ciso646>
#include <algorithm>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <vector>
#include <random>
#include <boost/sort/spinsort/spinsort.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/test_tools.hpp>

using namespace boost::sort;
using spin_detail::check_stable_sort;
using spin_detail::range_sort;
using common::range;

void test1 ( );
void test2 ( );
void test3 ( );
void test4 ( );

//---------------- stability test -----------------------------------
struct xk
{
    unsigned tail : 4;
    unsigned num : 28;
    xk ( uint32_t n =0 , uint32_t t =0): tail (t), num(n){};
    bool operator< (xk A) const { return (num < A.num); };
};

void test1 ( )
{
    typedef std::less< xk > compare_t;
    std::mt19937_64 my_rand (0);

    const uint32_t NMAX = 100000;

    std::vector< xk > V1, V2;
    V1.reserve (NMAX);
    for (uint32_t i = 0; i < 8; ++i) {
        for (uint32_t k = 0; k < NMAX; ++k) {
            uint32_t NM = my_rand ( );
            xk G;
            G.num = NM >> 3;
            G.tail = i;
            V1.push_back (G);
        };
    };
    V2 = V1;
    spinsort (V1.begin ( ), V1.end ( ), compare_t ( ));
    std::stable_sort (V2.begin ( ), V2.end ( ));

    BOOST_CHECK (V1.size ( ) == V2.size ( ));
    for (uint32_t i = 0; i < V1.size ( ); ++i) {
        BOOST_CHECK (V1[ i ].num == V2[ i ].num and
                     V1[ i ].tail == V2[ i ].tail);
    };
};

void test2 (void)
{
    typedef std::less< uint64_t > compare_t;

    const uint32_t NElem = 100000;
    std::vector< uint64_t > V1,V2;
    std::mt19937_64 my_rand (0);
    compare_t comp;

    // ------------------------ random elements -------------------------------
    for (uint32_t i = 0; i < NElem; ++i) V1.push_back (my_rand ( ) % NElem);
    V2 = V1;
    spinsort (V1.begin ( ), V1.end ( ), comp);
    std::stable_sort (V2.begin ( ), V2.end ( ), comp);
    for (unsigned i = 0; i < NElem; i++) {
        BOOST_CHECK (V2[ i ] == V1[ i ]);
    };

    // --------------------------- sorted elements ----------------------------
    V1.clear ( );
    for (uint32_t i = 0; i < NElem; ++i) V1.push_back (i);
    spinsort (V1.begin ( ), V1.end ( ), comp);
    for (unsigned i = 1; i < NElem; i++) {
        BOOST_CHECK (V1[ i - 1 ] <= V1[ i ]);
    };

    //-------------------------- reverse sorted elements ----------------------
    V1.clear ( );
    for (uint32_t i = 0; i < NElem; ++i) V1.push_back (NElem - i);
    spinsort (V1.begin ( ), V1.end ( ), comp);
    for (unsigned i = 1; i < NElem; i++) {
        BOOST_CHECK (V1[ i - 1 ] <= V1[ i ]);
    };

    //---------------------------- equal elements ----------------------------
    V1.clear ( );
    for (uint32_t i = 0; i < NElem; ++i) V1.push_back (1000);
    spinsort (V1.begin ( ), V1.end ( ), comp);
    for (unsigned i = 1; i < NElem; i++) {
        BOOST_CHECK (V1[ i - 1 ] == V1[ i ]);
    };
};

void test3 (void)
{
    typedef typename std::vector<uint64_t>::iterator    iter_t ;
    typedef range<iter_t>                               range_it ;
    std::less<uint64_t> comp ;

    std::vector<uint64_t> V = { 1,  3,  5,  7,  9, 11, 13, 15, 17, 19,
                               21, 23, 25, 27, 29, 31, 33, 35, 37, 39,
                               41, 43, 45, 47, 49, 51, 53, 55, 57, 59,
                               14,  2,  4,  6,  8, 10, 12, 16, 18, 20};
    range_it rdata (V.begin() , V.end());
    std::vector<uint64_t> aux (40,0 );
    range_it raux ( aux.begin() , aux.end());

    check_stable_sort ( rdata, raux, comp );
    for ( uint32_t i =0 ; i < V.size() ; ++i)
        std::cout<<V[i]<<", ";
    std::cout<<std::endl;

    V = {59, 57, 55, 53, 51, 49, 47, 45, 43, 41,
         39, 37, 35, 33, 31, 29, 27, 25, 23, 21,
         19, 17, 15, 13, 11,  9,  7,  5,  3,  1,
         14,  2,  6, 16, 18, 20, 12,  4,  8, 10};
    rdata = range_it (V.begin() , V.end());
    aux.assign (40,0);
    raux = range_it ( aux.begin() , aux.end());
    check_stable_sort ( rdata, raux, comp );
    for ( uint32_t i =0 ; i < V.size() ; ++i)
        std::cout<<V[i]<<", ";
    std::cout<<std::endl;

}
void test4 (void)
{
    typedef typename std::vector<xk>::iterator  iter_t;
    typedef std::less<xk>        compare_t;
    std::mt19937 my_rand (0);
    std::vector<xk> V ;
    const uint32_t NELEM = 100000;
    V.reserve(NELEM * 10);

    for (uint32_t k =0 ; k < 10 ; ++k)
    {   for ( uint32_t i =0 ; i < NELEM ; ++i)
        {   V.emplace_back(i , k);
        };
        iter_t first = V.begin() + (k * NELEM);
        iter_t last = first + NELEM ;
        std::shuffle( first, last, my_rand);
    };
    spinsort( V.begin() , V.end(), compare_t());
    for ( uint32_t i =0 ; i < ( NELEM * 10); ++i)
    {   BOOST_CHECK ( V[i].num == (i / 10) and V[i].tail == (i %10) );
    };
}
int test_main (int, char *[])
{
    test1 ( );
    test2 ( );
    test3 ( );
    test4 ( );
    return 0;
};


#include <chrono>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <memory>
#include <string>
#include <vector>
#include <boost/asio.hpp>
#include <asio-zmq.hpp>
#include "helper.hpp"

int main(int argc, char* argv[])
{
    if (argc != 4) {
        std::cerr << "usage: local_thr <bind-to> <message-size> <message-count>\n";
        return 1;
    }

    std::string const ep = argv[1];
    int message_size = std::atoi(argv[2]);
    int message_count = std::atoi(argv[3]);

    std::cout << "message size: " << message_size << " [B]\n";
    std::cout << "message count: " << message_count << "\n";

    boost::asio::io_service ios;
    boost::asio::zmq::context ctx;

    boost::asio::zmq::test::perf::puller pl(ios, ctx, message_count, ep);

    auto watch = std::chrono::system_clock::now();

    ios.run();

    auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(
                       std::chrono::system_clock::now() - watch).count();
    unsigned long throughput =
        static_cast<double>(message_count) / static_cast<double>(elapsed) * 1000000;
    double megabits = static_cast<double>(throughput * message_size * 8) / 1000000;

    std::cout << "mean throughput: " << throughput << " [msg/s]\n";
    std::cout << "mean throughput: " << megabits << " [Mb/s]\n";
}


/**
 * \file dcs/math/random/uniform_01_adaptor.hpp
 *
 * \brief Adaptor for generating random numbers in [0,1] from a base generator.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_RANDOM_UNIFORM_01_ADAPTOR_HPP
#define DCS_MATH_RANDOM_UNIFORM_01_ADAPTOR_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(101500) // 1.15
#	error "Required Boost library version >= 1.15"
#endif // DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION

#include <boost/random/uniform_01.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <dcs/math/random/generator_traits.hpp>
#include <dcs/type_traits/remove_reference.hpp>
#include <dcs/util/holder.hpp>
#include <limits>

namespace dcs { namespace math { namespace random {

/**
 * \brief Adaptor for generating random numbers in [0,1] from a base random
 *  number generator.
 *
 * \tparam BaseBaseRandomGeneratorT The base random number generator type.
 * \tparam RealT The result type.
 *
 * Wraps a random number generator of type \a BaseGeneratorT for generating
 * random numbers in [0,1].
 * Random numbers are generator according the algorithm provided by
 * \a BaseRandomGeneratorT.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename BaseRandomGeneratorT, typename RealT>
class uniform_01_adaptor: public base_generator<RealT>
{
	private: typedef base_generator<RealT> base_type;
    public: typedef BaseRandomGeneratorT base_engine_type;
    public: typedef typename ::dcs::type_traits::remove_reference<BaseRandomGeneratorT>::type::result_type input_type;
    public: typedef typename base_type::result_type result_type;
    public: typedef typename base_type::ulonglong_type ulonglong_type;
	private: typedef ::boost::random::uniform_01<result_type> adaptor_impl_type;

	/// Default constructor.
    public: uniform_01_adaptor()
        : base_type(),
		  rng_(),
		  impl_()
    {
		// empty
    }

	/// Seed constructor.
    public: uniform_01_adaptor(input_type seed)
        : base_type(),
		  rng_(seed),
		  impl_()
    {
		// empty
    }

	/// Copy constructor.
    public: uniform_01_adaptor(uniform_01_adaptor const& that)
        : base_type(that),
		  rng_(that.rng_),
		  impl_(that.impl_)
    {
		// empty
    }

	/// A Constructor: copy the base generator.
	public: uniform_01_adaptor(base_engine_type rng)
		: base_type(),
		  rng_(rng),
		  impl_()
	{
		// empty
	}

// Support of reference-to-refernce qualifier is only available from C++0x.
#if __cplusplus > 201103L

	/// A constructor: move the base generator.
    //public: uniform_01_adaptor(base_engine_type&& rng)
    public: uniform_01_adaptor(base_engine_type& rng)
        : base_type(),
		  rng_(rng),
		  impl_()
    {
		// empty
    }

#else

	/// A constructor: move the base generator.
    public: uniform_01_adaptor(::dcs::util::holder<base_engine_type>& rng)
        : base_type(),
		  rng_(rng),
		  impl_()
    {
		// empty
    }

#endif

	public: const base_engine_type base() const
	{
		return rng_;
	}

    private: result_type do_generate()
    {
//		result_type factor;
//		result_type result;
//
//		do
//		{
//			factor = result_type(1)
//					 / (
//							static_cast<result_type>(
//							(rng_.max)()-(rng_.min)()
//						)
//						+ static_cast<result_type>(
//							::std::numeric_limits<input_type>::is_integer
//							? 1
//							: 0
//						)
//			);
//			result = static_cast<result_type>(rng_()-(rng_.min)()) * factor;
//		}
//		while (result >= result_type(1));
//
//		return result;

		return impl_(rng_);
    }

	private: void do_seed()
	{
		this->rng_.seed();
	}

	private: void do_seed(result_type s)
	{
		rng_.seed(s);
		//generator_traits<base_engine_type>::seed(rng_, s);
	}

	private: void do_discard(ulonglong_type z)
	{
		while (z--)
		{
			rng_();
		}
	}

	private: result_type do_min() const
	{
		return result_type(0);
	}

	private: result_type do_max() const
	{
		return result_type(1);
	}

	private: base_engine_type rng_;
	private: adaptor_impl_type impl_;
};

// Template specialization for avoid auto-wrapping (i.e., when the base
// generator is itself a uniform_01_adaptor)
template <typename RandomNumberGeneratorT, typename RealT>
class uniform_01_adaptor< uniform_01_adaptor<RandomNumberGeneratorT,RealT>, RealT >: public uniform_01_adaptor<RandomNumberGeneratorT,RealT>
{
    public: typedef RandomNumberGeneratorT base_engine_type;
    public: typedef typename ::dcs::type_traits::remove_reference<RandomNumberGeneratorT>::type::result_type input_type;
    public: typedef RealT result_type;
};

}}} // Namespace dcs::math::random

#endif // DCS_MATH_RANDOM_UNIFORM_01_ADAPTOR_HPP


#include <NTL/lzz_p.h>

#include <NTL/new.h>

NTL_START_IMPL

zz_pInfoT::zz_pInfoT(long NewP, long maxroot)
{
   ref_count = 1;

   if (maxroot < 0) Error("zz_pContext: maxroot may not be negative");

   if (NewP <= 1) Error("zz_pContext: p must be > 1");
   if (NumBits(NewP) > NTL_SP_NBITS) Error("zz_pContext: modulus too big");

   ZZ P, B, M, M1, MinusM;
   long n, i;
   long q, t;

   p = NewP;

   pinv = 1/double(p);

   index = -1;

   conv(P, p);

   sqr(B, P);
   LeftShift(B, B, maxroot+NTL_FFTFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseFFTPrime(n);
      q = FFTPrime[n];
      n++;
      mul(M, M, q);
   }

   if (n > 4) Error("zz_pInit: too many primes");

   NumPrimes = n;
   PrimeCnt = n;
   MaxRoot = CalcMaxRoot(q);

   if (maxroot < MaxRoot)
      MaxRoot = maxroot;

   negate(MinusM, M);
   MinusMModP = rem(MinusM, p);

   if (!(CoeffModP = (long *) NTL_MALLOC(n, sizeof(long), 0)))
      Error("out of space");

   if (!(x = (double *) NTL_MALLOC(n, sizeof(double), 0)))
      Error("out of space");

   if (!(u = (long *) NTL_MALLOC(n, sizeof(long), 0)))
      Error("out of space");

   for (i = 0; i < n; i++) {
      q = FFTPrime[i];

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q);
      mul(M1, M1, t);
      CoeffModP[i] = rem(M1, p);
      x[i] = ((double) t)/((double) q);
      u[i] = t;
   }
}

zz_pInfoT::zz_pInfoT(long Index)
{
   ref_count = 1;

   index = Index;

   if (index < 0)
      Error("bad FFT prime index");

   // allows non-consecutive indices...I'm not sure why
   while (NumFFTPrimes < index)
      UseFFTPrime(NumFFTPrimes);

   UseFFTPrime(index);

   p = FFTPrime[index];
   pinv = FFTPrimeInv[index];

   NumPrimes = 1;
   PrimeCnt = 0;

   MaxRoot = CalcMaxRoot(p);
}

zz_pInfoT::~zz_pInfoT()
{
   if (index < 0) {
      free(CoeffModP);
      free(x);
      free(u);
   }
}

zz_pInfoT *zz_pInfo = 0;

typedef zz_pInfoT *zz_pInfoPtr;

static
void CopyPointer(zz_pInfoPtr& dst, zz_pInfoPtr src)
{
   if (src == dst) return;

   if (dst) {
      dst->ref_count--;

      if (dst->ref_count < 0)
         Error("internal error: negative zz_pContext ref_count");

      if (dst->ref_count == 0) delete dst;
   }

   if (src) {
      if (src->ref_count == NTL_MAX_LONG)
         Error("internal error: zz_pContext ref_count overflow");

      src->ref_count++;
   }

   dst = src;
}

void zz_p::init(long p, long maxroot)
{
   zz_pContext c(p, maxroot);
   c.restore();

}

void zz_p::FFTInit(long index)
{
   zz_pContext c(INIT_FFT, index);
   c.restore();
}

zz_pContext::zz_pContext(long p, long maxroot)
{
   ptr = NTL_NEW_OP zz_pInfoT(p, maxroot);
}

zz_pContext::zz_pContext(INIT_FFT_TYPE, long index)
{
   ptr = NTL_NEW_OP zz_pInfoT(index);
}

zz_pContext::zz_pContext(const zz_pContext& a)
{
   ptr = 0;
   CopyPointer(ptr, a.ptr);
}

zz_pContext& zz_pContext::operator=(const zz_pContext& a)
{
   CopyPointer(ptr, a.ptr);
   return *this;
}

zz_pContext::~zz_pContext()
{
   CopyPointer(ptr, 0);
}

void zz_pContext::save()
{
   CopyPointer(ptr, zz_pInfo);
}

void zz_pContext::restore() const
{
   CopyPointer(zz_pInfo, ptr);
}

zz_pBak::~zz_pBak()
{
   if (MustRestore)
      CopyPointer(zz_pInfo, ptr);

   CopyPointer(ptr, 0);
}

void zz_pBak::save()
{
   MustRestore = 1;
   CopyPointer(ptr, zz_pInfo);
}

void zz_pBak::restore()
{
   MustRestore = 0;
   CopyPointer(zz_pInfo, ptr);
}

static inline
long reduce(long a, long p)
{
   if (a >= 0 && a < p)
      return a;
   else {
      a = a % p;
      if (a < 0) a += p;
      return a;
   }
}

zz_p to_zz_p(long a)
{
   return zz_p(reduce(a, zz_p::modulus()), INIT_LOOP_HOLE);
}

void conv(zz_p& x, long a)
{
   x._zz_p__rep = reduce(a, zz_p::modulus());
}

zz_p to_zz_p(const ZZ& a)
{
   return zz_p(rem(a, zz_p::modulus()), INIT_LOOP_HOLE);
}

void conv(zz_p& x, const ZZ& a)
{
   x._zz_p__rep = rem(a, zz_p::modulus());
}

istream& operator>>(istream& s, zz_p& x)
{
   static ZZ y;
   s >> y;
   conv(x, y);

   return s;
}

ostream& operator<<(ostream& s, zz_p a)
{
   static ZZ y;
   y = rep(a);
   s << y;

   return s;
}

NTL_END_IMPL