// MSVC has a bug where it chokes on complex Eigen expressions

#ifdef _MSC_VER
#define EIGEN_STRONG_INLINE inline
#endif

#include "MidedgeAngleSinFormulation.h"
#include "../GeometryDerivatives.h"
#include "../MeshConnectivity.h"
#include <iostream>
#include <random>
#include <Eigen/Geometry>

static double edgeTheta(
    const MeshConnectivity& mesh,
    const Eigen::MatrixXd& curPos,
    int edge,
    Eigen::Matrix<double, 1, 12>* derivative, // edgeVertex, then edgeOppositeVertex
    Eigen::Matrix<double, 12, 12>* hessian)
{
    if (derivative)
        derivative->setZero();
    if (hessian)
        hessian->setZero();
    int v0 = mesh.edgeVertex(edge, 0);
    int v1 = mesh.edgeVertex(edge, 1);
    int v2 = mesh.edgeOppositeVertex(edge, 0);
    int v3 = mesh.edgeOppositeVertex(edge, 1);
    if (v2 == -1 || v3 == -1)
        return 0; // boundary edge

    Eigen::Vector3d q0 = curPos.row(v0);
    Eigen::Vector3d q1 = curPos.row(v1);
    Eigen::Vector3d q2 = curPos.row(v2);
    Eigen::Vector3d q3 = curPos.row(v3);

    Eigen::Vector3d n0 = (q0 - q2).cross(q1 - q2);
    Eigen::Vector3d n1 = (q1 - q3).cross(q0 - q3);
    Eigen::Vector3d axis = q1 - q0;
    Eigen::Matrix<double, 1, 9> angderiv;
    Eigen::Matrix<double, 9, 9> anghess;

    double theta = angle(n0, n1, axis, (derivative || hessian) ? &angderiv : NULL, hessian ? &anghess : NULL);

    if (derivative)
    {
        derivative->block<1, 3>(0, 0) += angderiv.block<1, 3>(0, 0) * crossMatrix(q2 - q1);
        derivative->block<1, 3>(0, 3) += angderiv.block<1, 3>(0, 0) * crossMatrix(q0 - q2);
        derivative->block<1, 3>(0, 6) += angderiv.block<1, 3>(0, 0) * crossMatrix(q1 - q0);

        derivative->block<1, 3>(0, 0) += angderiv.block<1, 3>(0, 3) * crossMatrix(q1 - q3);
        derivative->block<1, 3>(0, 3) += angderiv.block<1, 3>(0, 3) * crossMatrix(q3 - q0);
        derivative->block<1, 3>(0, 9) += angderiv.block<1, 3>(0, 3) * crossMatrix(q0 - q1);
    }

    if (hessian)
    {
        Eigen::Matrix3d vqm[3];
        vqm[0] = crossMatrix(q0 - q2);
        vqm[1] = crossMatrix(q1 - q0);
        vqm[2] = crossMatrix(q2 - q1);
        Eigen::Matrix3d wqm[3];
        wqm[0] = crossMatrix(q0 - q1);
        wqm[1] = crossMatrix(q1 - q3);
        wqm[2] = crossMatrix(q3 - q0);

        int vindices[3] = { 3, 6, 0 };
        int windices[3] = { 9, 0, 3 };

        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                hessian->block<3, 3>(vindices[i], vindices[j]) += vqm[i].transpose() * anghess.block<3, 3>(0, 0) * vqm[j];
                hessian->block<3, 3>(vindices[i], windices[j]) += vqm[i].transpose() * anghess.block<3, 3>(0, 3) * wqm[j];
                hessian->block<3, 3>(windices[i], vindices[j]) += wqm[i].transpose() * anghess.block<3, 3>(3, 0) * vqm[j];
                hessian->block<3, 3>(windices[i], windices[j]) += wqm[i].transpose() * anghess.block<3, 3>(3, 3) * wqm[j];
            }

            hessian->block<3, 3>(vindices[i], 3) += vqm[i].transpose() * anghess.block<3, 3>(0, 6);
            hessian->block<3, 3>(3, vindices[i]) += anghess.block<3, 3>(6, 0) * vqm[i];
            hessian->block<3, 3>(vindices[i], 0) += -vqm[i].transpose() * anghess.block<3, 3>(0, 6);
            hessian->block<3, 3>(0, vindices[i]) += -anghess.block<3, 3>(6, 0) * vqm[i];

            hessian->block<3, 3>(windices[i], 3) += wqm[i].transpose() * anghess.block<3, 3>(3, 6);
            hessian->block<3, 3>(3, windices[i]) += anghess.block<3, 3>(6, 3) * wqm[i];
            hessian->block<3, 3>(windices[i], 0) += -wqm[i].transpose() * anghess.block<3, 3>(3, 6);
            hessian->block<3, 3>(0, windices[i]) += -anghess.block<3, 3>(6, 3) * wqm[i];

        }

        Eigen::Vector3d dang1 = angderiv.block<1, 3>(0, 0).transpose();
        Eigen::Vector3d dang2 = angderiv.block<1, 3>(0, 3).transpose();

        Eigen::Matrix3d dang1mat = crossMatrix(dang1);
        Eigen::Matrix3d dang2mat = crossMatrix(dang2);

        hessian->block<3, 3>(6, 3) += dang1mat;
        hessian->block<3, 3>(0, 3) -= dang1mat;
        hessian->block<3, 3>(0, 6) += dang1mat;
        hessian->block<3, 3>(3, 0) += dang1mat;
        hessian->block<3, 3>(3, 6) -= dang1mat;
        hessian->block<3, 3>(6, 0) -= dang1mat;

        hessian->block<3, 3>(9, 0) += dang2mat;
        hessian->block<3, 3>(3, 0) -= dang2mat;
        hessian->block<3, 3>(3, 9) += dang2mat;
        hessian->block<3, 3>(0, 3) += dang2mat;
        hessian->block<3, 3>(0, 9) -= dang2mat;
        hessian->block<3, 3>(9, 3) -= dang2mat;
    }

    return theta;
}

static Eigen::Vector3d secondFundamentalFormEntries(
    const MeshConnectivity& mesh,
    const Eigen::MatrixXd& curPos,
    const Eigen::VectorXd& edgeThetas,
    int face,
    Eigen::Matrix<double, 3, 21>* derivative,
    std::vector<Eigen::Matrix<double, 21, 21> >* hessian)
{
    if (derivative)
        derivative->setZero();
    if (hessian)
    {
        hessian->resize(3);
        for (int i = 0; i < 3; i++)
            (*hessian)[i].setZero();
    }

    Eigen::Vector3d II;
    for (int i = 0; i < 3; i++)
    {
        Eigen::Matrix<double, 1, 9> hderiv;
        Eigen::Matrix<double, 9, 9> hhess;
        double altitude = triangleAltitude(mesh, curPos, face, i, (derivative || hessian) ? &hderiv : NULL, hessian ? &hhess : NULL);

        int edge = mesh.faceEdge(face, i);
        Eigen::Matrix<double, 1, 12> thetaderiv;
        Eigen::Matrix<double, 12, 12> thetahess;
        double theta = edgeTheta(mesh, curPos, edge, (derivative || hessian) ? &thetaderiv : NULL, hessian ? &thetahess : NULL);

        double orient = mesh.faceEdgeOrientation(face, i) == 0 ? 1.0 : -1.0;
        double alpha = 0.5 * theta + orient * edgeThetas[edge];

        double sinAlpha = sin(alpha);
        double cosAlpha = cos(alpha);

        II[i] = 2.0 * altitude * sinAlpha;

        if (derivative)
        {
            int hv0 = i;
            int hv1 = (i + 1) % 3;
            int hv2 = (i + 2) % 3;
            derivative->block<1, 3>(i, 3 * hv0) += 2.0 * sinAlpha * hderiv.block<1, 3>(0, 0);
            derivative->block<1, 3>(i, 3 * hv1) += 2.0 * sinAlpha * hderiv.block<1, 3>(0, 3);
            derivative->block<1, 3>(i, 3 * hv2) += 2.0 * sinAlpha * hderiv.block<1, 3>(0, 6);

            int av0, av1, av2, av3;
            if (mesh.faceEdgeOrientation(face, i) == 0)
            {
                av0 = (i + 1) % 3;
                av1 = (i + 2) % 3;
                av2 = i;
                av3 = 3 + i;
            }
            else
            {
                av0 = (i + 2) % 3;
                av1 = (i + 1) % 3;
                av2 = 3 + i;
                av3 = i;
            }
            derivative->block<1, 3>(i, 3 * av0) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 0);
            derivative->block<1, 3>(i, 3 * av1) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 3);
            derivative->block<1, 3>(i, 3 * av2) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 6);
            derivative->block<1, 3>(i, 3 * av3) += altitude * cosAlpha * thetaderiv.block<1, 3>(0, 9);
            (*derivative)(i, 18 + i) += 2.0 * altitude * cosAlpha * orient;
        }

        if (hessian)
        {
            int hv[3];
            hv[0] = i;
            hv[1] = (i + 1) % 3;
            hv[2] = (i + 2) % 3;
            for (int j = 0; j < 3; j++)
            {
                for (int k = 0; k < 3; k++)
                {
                    (*hessian)[i].block<3, 3>(3 * hv[j], 3 * hv[k]) += 2.0 * sinAlpha * hhess.block<3, 3>(3 * j, 3 * k);
                }
            }

            int av[4];
            if (mesh.faceEdgeOrientation(face, i) == 0)
            {
                av[0] = (i + 1) % 3;
                av[1] = (i + 2) % 3;
                av[2] = i;
                av[3] = 3 + i;
            }
            else
            {
                av[0] = (i + 2) % 3;
                av[1] = (i + 1) % 3;
                av[2] = 3 + i;
                av[3] = i;
            }

            for (int k = 0; k < 3; k++)
            {
                for (int j = 0; j < 4; j++)
                {
                    (*hessian)[i].block<3, 3>(3 * av[j], 3 * hv[k]) += cosAlpha * thetaderiv.block<1, 3>(0, 3 * j).transpose() * hderiv.block<1, 3>(0, 3 * k);
                    (*hessian)[i].block<3, 3>(3 * hv[k], 3 * av[j]) += cosAlpha * hderiv.block<1, 3>(0, 3 * k).transpose() * thetaderiv.block<1, 3>(0, 3 * j);
                }
                (*hessian)[i].block<1, 3>(18 + i, 3 * hv[k]) += 2.0 * cosAlpha * orient * hderiv.block<1, 3>(0, 3 * k);
                (*hessian)[i].block<3, 1>(3 * hv[k], 18 + i) += 2.0 * cosAlpha * orient * hderiv.block<1, 3>(0, 3 * k).transpose();
            }

            for (int k = 0; k < 4; k++)
            {
                for (int j = 0; j < 4; j++)
                {
                    (*hessian)[i].block<3, 3>(3 * av[j], 3 * av[k]) += altitude * cosAlpha * thetahess.block<3, 3>(3 * j, 3 * k);
                    (*hessian)[i].block<3, 3>(3 * av[j], 3 * av[k]) += -0.5 * altitude * sinAlpha * thetaderiv.block<1, 3>(0, 3 * j).transpose() * thetaderiv.block<1, 3>(0, 3 * k);
                }
                (*hessian)[i].block<1, 3>(18 + i, 3 * av[k]) += -1.0 * altitude * sinAlpha * orient * thetaderiv.block<1, 3>(0, 3 * k);
                (*hessian)[i].block<3, 1>(3 * av[k], 18 + i) += -1.0 * altitude * sinAlpha * orient * thetaderiv.block<1, 3>(0, 3 * k).transpose();
            }

            (*hessian)[i](18 + i, 18 + i) += -2.0 * altitude * sinAlpha;
        }
    }

    return II;
}

Eigen::Matrix2d MidedgeAngleSinFormulation::secondFundamentalForm(
    const MeshConnectivity &mesh,
    const Eigen::MatrixXd &curPos,
    const Eigen::VectorXd &extraDOFs,
    int face,
    Eigen::MatrixXd *derivative,
    std::vector<Eigen::MatrixXd> *hessian) const
{
    if (derivative)
    {
        derivative->resize(4, 21);
        derivative->setZero();
    }
    if (hessian)
    {
        hessian->resize(4);
        for (int i = 0; i < 4; i++)
        {
            (*hessian)[i].resize(21, 21);
            (*hessian)[i].setZero();
        }
    }

    Eigen::Matrix<double, 3, 21> IIderiv;
    std::vector < Eigen::Matrix<double, 21, 21> > IIhess;

    Eigen::Vector3d II = secondFundamentalFormEntries(mesh, curPos, extraDOFs, face, derivative ? &IIderiv : NULL, hessian ? &IIhess : NULL);

    Eigen::Matrix2d result;
    result << II[0] + II[1], II[0], II[0], II[0] + II[2];

    if (derivative)
    {
        derivative->row(0) += IIderiv.row(0);
        derivative->row(0) += IIderiv.row(1);

        derivative->row(1) += IIderiv.row(0);
        derivative->row(2) += IIderiv.row(0);

        derivative->row(3) += IIderiv.row(0);
        derivative->row(3) += IIderiv.row(2);
    }
    if (hessian)
    {
        (*hessian)[0] += IIhess[0];
        (*hessian)[0] += IIhess[1];

        (*hessian)[1] += IIhess[0];
        (*hessian)[2] += IIhess[0];

        (*hessian)[3] += IIhess[0];
        (*hessian)[3] += IIhess[2];
    }

    return result;
}

static void testSecondFundamentalFormEntries(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F)
{
    double eps = 1e-6;
    MeshConnectivity mesh(F);
    int nfaces = mesh.nFaces();
    int nedges = mesh.nEdges();
    Eigen::VectorXd thetas(nedges);
    thetas.setConstant(0.3);
    int ntests = 100;

    std::random_device rd;
    std::mt19937 rng(rd());
    std::uniform_int_distribution<int> uni(0, nfaces);

    std::cout << "Testing " << ntests << " random faces" << std::endl;

    for(int i=0; i<ntests; i++)
    {
        int face = uni(rng);
        Eigen::Matrix<double, 3, 21> deriv;
        std::vector<Eigen::Matrix<double, 21, 21> > hess;
        Eigen::Vector3d b = secondFundamentalFormEntries(mesh, V, thetas, face, &deriv, &hess);

        for(int j=0; j<3; j++)
        {
            for(int k=0; k<3; k++)
            {
                Eigen::MatrixXd Vpert(V);
                Vpert(mesh.faceVertex(face, j), k) += 1e-6;
                Eigen::Matrix<double, 3, 21> derivpert;
                Eigen::Vector3d bpert = secondFundamentalFormEntries(mesh, Vpert, thetas, face, &derivpert, NULL);
                Eigen::Vector3d findiff = (bpert-b)/1e-6;
                Eigen::Vector3d exact = deriv.col(3*j+k);
                std::cout << "q" << j << "[" << k <<"]: " << exact.transpose() << " / " << findiff.transpose() << std::endl;

                Eigen::Matrix<double, 3, 21> findiffhess = (derivpert-deriv)/1e-6;
                for(int l=0; l<3; l++)
                {
                    std::cout << " hess[" << l << "]: " << hess[l].col(3*j+k).transpose() << " / " << findiffhess.row(l) << std::endl;;
                }
            }
            int edge = mesh.faceEdge(face, j);
            int ofaceidx = 0;
            if(mesh.edgeFace(edge, ofaceidx) == face)
                ofaceidx = 1;
            if(mesh.edgeFace(edge, ofaceidx) == -1)
                continue;
            int pidx = mesh.edgeOppositeVertex(edge, ofaceidx);
            for(int k=0; k<3; k++)
            {
                Eigen::MatrixXd Vpert(V);
                Vpert(pidx, k) += 1e-6;
                Eigen::Matrix<double, 3, 21> derivpert;
                Eigen::Vector3d bpert = secondFundamentalFormEntries(mesh, Vpert, thetas, face, &derivpert, NULL);
                Eigen::Vector3d findiff = (bpert-b)/1e-6;
                Eigen::Vector3d exact = deriv.col(9 + 3*j+k);
                std::cout << "p" << j << "[" << k <<"]: " << exact.transpose() << " / " << findiff.transpose() << std::endl;

                Eigen::Matrix<double, 3, 21> findiffhess = (derivpert-deriv)/1e-6;
                for(int l=0; l<3; l++)
                {
                    std::cout << " hess[" << l << "]: " << hess[l].col(9 + 3*j+k).transpose() << " / " << findiffhess.row(l) << std::endl;;
                }
            }
            Eigen::VectorXd thetapert(thetas);
            thetapert[edge] += 1e-6;
            Eigen::Matrix<double, 3, 21> derivpert;
            Eigen::Vector3d bpert = secondFundamentalFormEntries(mesh, V, thetapert, face, &derivpert, NULL);
            Eigen::Vector3d findiff = (bpert-b)/1e-6;
            Eigen::Vector3d exact = deriv.col(18+j);
            std::cout << "theta[" << j << "]: " << exact.transpose() << " / " << findiff.transpose() << std::endl;

            Eigen::Matrix<double, 3, 21> findiffhess = (derivpert-deriv)/1e-6;
            for(int l=0; l<3; l++)
            {
                std::cout << " hess[" << l << "]: " << hess[l].col(18 + j).transpose() << " / " << findiffhess.row(l) << std::endl;;
            }
        }
    }
}

static void testThetas(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F)
{
    double eps = 1e-6;
    MeshConnectivity mesh(F);
    int nfaces = mesh.nFaces();
    int nedges = mesh.nEdges();
    int ntests = 100;

    std::random_device rd;
    std::mt19937 rng(rd());
    std::uniform_int_distribution<int> uni(0, nedges);

    std::cout << "Testing " << ntests << " random edges" << std::endl;

    for (int i = 0; i < ntests; i++)
    {
        int edge = uni(rng);
        Eigen::Matrix<double, 1, 12> deriv;
        Eigen::Matrix<double, 12, 12> hess;
        double theta = edgeTheta(mesh, V, edge, &deriv, &hess);

        int v[2];
        v[0] = mesh.edgeVertex(edge, 0);
        v[1] = mesh.edgeVertex(edge, 1);

        int vo[2];
        vo[0] = mesh.edgeOppositeVertex(edge, 0);
        vo[1] = mesh.edgeOppositeVertex(edge, 1);

        for (int j = 0; j < 2; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                Eigen::MatrixXd Vpert(V);
                Vpert(v[j], k) += 1e-6;
                Eigen::Matrix<double, 1, 12> derivpert;
                double thetapert = edgeTheta(mesh, Vpert, edge, &derivpert, NULL);
                double findiff = (thetapert - theta) / 1e-6;
                double exact = deriv(0, 3 * j + k);
                std::cout << "edgeVert" << j << "[" << k << "]: " << exact << " / " << findiff << std::endl;

                Eigen::Matrix<double, 1, 12> findiffhess = (derivpert-deriv)/1e-6;
                std::cout << " hess: " << hess.col(3 * j + k).transpose() << " / " << findiffhess << std::endl;
            }

            if (vo[j] != -1)
            {
                for (int k = 0; k < 3; k++)
                {
                    Eigen::MatrixXd Vpert(V);
                    Vpert(vo[j], k) += 1e-6;
                    Eigen::Matrix<double, 1, 12> derivpert;
                    double thetapert = edgeTheta(mesh, Vpert, edge, &derivpert, NULL);
                    double findiff = (thetapert - theta) / 1e-6;
                    double exact = deriv(0, 6 + 3 * j + k);
                    std::cout << "edgeOppVert" << j << "[" << k << "]: " << exact << " / " << findiff << std::endl;

                    Eigen::Matrix<double, 1, 12> findiffhess = (derivpert-deriv)/1e-6;
                    std::cout << " hess: " << hess.col(6 + 3 * j + k).transpose() << " / " << findiffhess << std::endl;
                }
            }
        }
    }
}

int MidedgeAngleSinFormulation::numExtraDOFs() const
{
    return 1;
}

void MidedgeAngleSinFormulation::initializeExtraDOFs(Eigen::VectorXd &extraDOFs, const MeshConnectivity &mesh, const Eigen::MatrixXd &curPos) const
{
    extraDOFs.resize(mesh.nEdges());
    extraDOFs.setZero();
}


#define BOOST_TEST_MODULE "dsn::parallel_for"

#include <array>
#include <chrono>
#include <cmath>
#include <iostream>

#include <dsnutil/parallel_for.h>

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

using Clock = std::chrono::high_resolution_clock;
using TimePoint = Clock::time_point;

using std::chrono::duration_cast;
using std::chrono::milliseconds;

BOOST_AUTO_TEST_CASE(parallel_for)
{
    static const size_t size{ static_cast<size_t>(1e4) };
    std::array<double, size> result;
    std::fill(result.begin(), result.end(), 0.0);

    auto f = [](size_t i) -> double {
        return std::sin(2.0 * boost::math::constants::pi<double>() / static_cast<double>(i + 1));
    };

    std::cout << "launching parallel_for loop for " << size << " elements..." << std::endl;
    TimePoint parallel_start = Clock::now();
    dsn::parallel_for(size, [&](size_t index) { result[index] = f(index); });
    TimePoint parallel_end = Clock::now();
    auto parallel_time = duration_cast<milliseconds>(parallel_end - parallel_start).count();
    std::cout << "parallel_for loop execution finished in " << parallel_time << "ms" << std::endl;
}


#define BOOST_TEST_MODULE "test_vector"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <boost/mpl/list.hpp>
#include <mjolnir/math/Vector.hpp>

#include <random>
#include <cstdint>

constexpr std::uint32_t seed = 123456789;
constexpr std::size_t   N    = 10000;
typedef boost::mpl::list<double, float> test_targets;

namespace test
{
template<typename T>
decltype(boost::test_tools::tolerance(std::declval<T>())) tolerance();

template<>
decltype(boost::test_tools::tolerance(std::declval<float>()))
tolerance<float>()
{return boost::test_tools::tolerance(3.0f / static_cast<float>(std::pow(2, 12)));}

template<>
decltype(boost::test_tools::tolerance(std::declval<double>()))
tolerance<double>()
{return boost::test_tools::tolerance(2.0 / std::pow(2, 14));}
} // test

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_construction, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    math::Vector<Real, 3> vec(1.0, 2.0, 3.0);
    BOOST_TEST(X(vec) == 1.0);
    BOOST_TEST(Y(vec) == 2.0);
    BOOST_TEST(Z(vec) == 3.0);

    X(vec) = 4.0;
    Y(vec) = 5.0;
    Z(vec) = 6.0;

    BOOST_TEST(X(vec) == 4.0);
    BOOST_TEST(Y(vec) == 5.0);
    BOOST_TEST(Z(vec) == 6.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_add, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-100.0, 100.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt)),
                              rhs(uni(mt), uni(mt), uni(mt));
        const auto add = lhs + rhs;
        BOOST_TEST(X(add) == X(lhs) + X(rhs), test::tolerance<Real>());
        BOOST_TEST(Y(add) == Y(lhs) + Y(rhs), test::tolerance<Real>());
        BOOST_TEST(Z(add) == Z(lhs) + Z(rhs), test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_sub, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-100.0, 100.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt)),
                              rhs(uni(mt), uni(mt), uni(mt));
        const auto sub = lhs - rhs;
        BOOST_TEST(X(sub) == X(lhs) - X(rhs), test::tolerance<Real>());
        BOOST_TEST(Y(sub) == Y(lhs) - Y(rhs), test::tolerance<Real>());
        BOOST_TEST(Z(sub) == Z(lhs) - Z(rhs), test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_mul, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-100.0, 100.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const Real            lhs(uni(mt));
        const math::Vector<Real, 3> rhs(uni(mt), uni(mt), uni(mt));
        const auto mul = lhs * rhs;
        BOOST_TEST(X(mul) == lhs * X(rhs), test::tolerance<Real>());
        BOOST_TEST(Y(mul) == lhs * Y(rhs), test::tolerance<Real>());
        BOOST_TEST(Z(mul) == lhs * Z(rhs), test::tolerance<Real>());
    }

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt));
        const Real            rhs(uni(mt));
        const auto mul = lhs * rhs;
        BOOST_TEST(X(mul) == X(lhs) * rhs, test::tolerance<Real>());
        BOOST_TEST(Y(mul) == Y(lhs) * rhs, test::tolerance<Real>());
        BOOST_TEST(Z(mul) == Z(lhs) * rhs, test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_div, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-100.0, 100.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt));
        const Real            rhs(uni(mt));
        const auto div = lhs / rhs;
        BOOST_TEST(X(div) == X(lhs) / rhs, test::tolerance<Real>());
        BOOST_TEST(Y(div) == Y(lhs) / rhs, test::tolerance<Real>());
        BOOST_TEST(Z(div) == Z(lhs) / rhs, test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_dot, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt));
        const math::Vector<Real, 3> rhs(uni(mt), uni(mt), uni(mt));
        const Real dot = math::dot_product(lhs, rhs);
        BOOST_TEST(dot == X(lhs) * X(rhs) + Y(lhs) * Y(rhs) + Z(lhs) * Z(rhs),
                   test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_len, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt));
        const Real dot   = math::dot_product(lhs, lhs);
        const Real lensq = math::length_sq(lhs);
        const Real len   = math::length(lhs);
        BOOST_TEST(lensq == dot, test::tolerance<Real>());

        BOOST_TEST(lensq == dot,            test::tolerance<Real>());
        BOOST_TEST(len   == std::sqrt(dot), test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_rlen, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt));
        const Real len  = math::length(lhs);
        const Real rlen = math::rlength(lhs);
        BOOST_TEST(rlen * len == Real(1.0), test::tolerance<Real>());
        BOOST_TEST(rlen == Real(1) / len,   test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_cross_product, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(-1.0, 1.0);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> lhs(uni(mt), uni(mt), uni(mt));
        const math::Vector<Real, 3> rhs(uni(mt), uni(mt), uni(mt));
        const math::Vector<Real, 3> cross = math::cross_product(lhs, rhs);
        const Real dotl = math::dot_product(cross, lhs);
        const Real dotr = math::dot_product(cross, rhs);

        BOOST_TEST(dotl == static_cast<Real>(0.0), test::tolerance<Real>());
        BOOST_TEST(dotr == static_cast<Real>(0.0), test::tolerance<Real>());
        const Real lenc = math::length(cross);

        const Real lenl = math::length(lhs);
        const Real lenr = math::length(rhs);
        const Real dot  = math::dot_product(lhs, rhs);
        const Real cost = dot / (lenl * lenr);
        const Real sint = std::sqrt(1.0 - cost * cost);

        BOOST_TEST(lenc == lenl * lenr * sint, test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_vector_tensor_product, Real, test_targets)
{
    using namespace mjolnir;
    using mjolnir::math::X;
    using mjolnir::math::Y;
    using mjolnir::math::Z;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<Real> uni(0, 1);

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> v1(uni(mt), uni(mt), uni(mt));
        const math::Vector<Real, 3> v2(uni(mt), uni(mt), uni(mt));
        const math::Vector<Real, 3> v3(uni(mt), uni(mt), uni(mt));
        const auto t1 = math::tensor_product(v1, v3);
        const auto t2 = math::tensor_product(v2, v3);
        const auto t3 = math::tensor_product(v1 + v2, v3);

        static_assert(std::is_same<math::Matrix<Real, 3, 3>,
                typename std::remove_const<decltype(t1)>::type>::value, "");

        for(std::size_t i=0; i<9; ++i)
        {
            BOOST_TEST(t1.at(i) + t2.at(i) == t3.at(i), test::tolerance<Real>());
        }
        const auto t4 = math::tensor_product(v1, v2);
        const auto t5 = math::tensor_product(v1, v3);
        const auto t6 = math::tensor_product(v1, v2 + v3);
        for(std::size_t i=0; i<9; ++i)
        {
            BOOST_TEST(t4.at(i) + t5.at(i) == t6.at(i), test::tolerance<Real>());
        }
        const auto t7 = math::tensor_product(2 * v1, v2);
        for(std::size_t i=0; i<9; ++i)
        {
            BOOST_TEST(2 * t4.at(i) == t7.at(i), test::tolerance<Real>());
        }
    }

    for(std::size_t test_times=0; test_times<N; ++test_times)
    {
        const math::Vector<Real, 3> v1(uni(mt), uni(mt), uni(mt));
        const math::Vector<Real, 3> v2(uni(mt), uni(mt), uni(mt));
        const auto t1 = math::tensor_product(v1, v2);
        const auto t2 = v1 * math::transpose(v2);
        for(std::size_t i=0; i<9; ++i)
        {
            BOOST_TEST(t1.at(i) == t2.at(i), test::tolerance<Real>());
        }
    }
}


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

/*
 Copyright (C) 2009 Andrea Odetti

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

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

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

#ifndef quantlib_longstaff_schwartz_multi_path_pricer_hpp
#define quantlib_longstaff_schwartz_multi_path_pricer_hpp

#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/math/functional.hpp>
#include <ql/methods/montecarlo/pathpricer.hpp>
#include <ql/methods/montecarlo/multipath.hpp>
#include <ql/methods/montecarlo/lsmbasissystem.hpp>
#include <ql/experimental/mcbasket/pathpayoff.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    //! Longstaff-Schwarz path pricer for early exercise options
    /*! References:

        Francis Longstaff, Eduardo Schwartz, 2001. Valuing American Options
        by Simulation: A Simple Least-Squares Approach, The Review of
        Financial Studies, Volume 14, No. 1, 113-147

        \ingroup mcarlo

        \test the correctness of the returned value is tested by
              reproducing results available in web/literature
    */
    class LongstaffSchwartzMultiPathPricer : public PathPricer<MultiPath> {
      public:

        LongstaffSchwartzMultiPathPricer(
            const boost::shared_ptr<PathPayoff>& ,
            const std::vector<Size> &,
            const std::vector<Handle<YieldTermStructure> > &,
            const Array &,
            Size ,
            LsmBasisSystem::PolynomType );

        Real operator()(const MultiPath& multiPath) const;
        virtual void calibrate();

      protected:
        struct PathInfo {
            PathInfo(Size numberOfTimes);

            Size pathLength() const;

            Array                   payments;
            Array                   exercises;
            std::vector<Array>      states;
        };

        PathInfo transformPath(const MultiPath& path) const;

        bool  calibrationPhase_;

        const boost::shared_ptr<PathPayoff> payoff_;

        boost::scoped_array<Array> coeff_;
        boost::scoped_array<Real> lowerBounds_;

        const std::vector<Size> timePositions_;
        const std::vector<Handle<YieldTermStructure> > forwardTermStructures_;
        const Array dF_;

        mutable std::vector<PathInfo> paths_;
        const   std::vector<boost::function1<Real, Array> > v_;
    };

}

#endif


#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

#include "LSLOpt/BFGS.hpp"

#include "ModelSystem.hpp"

int main(int argc, char* argv[])
{
  ModelSystem modelSystem;

  std::cerr << std::setprecision(16);

  double angle_eps = 0.1 * M_PI / 180.0;

  unsigned n_steps = static_cast<unsigned>((2 * M_PI) / angle_eps) + 1;

  for (unsigned i = 0; i < n_steps; ++i) {
    double angle = angle_eps * i;
    double x = modelSystem.x0.norm() * std::sin(angle);
    double y = modelSystem.x0.norm() * std::cos(angle);
    Eigen::VectorXd x0 = Eigen::VectorXd::Zero(1);
    x0[0] = angle;

    double val = modelSystem.value(x0);

    std::cerr << angle << ";" << x << ";" << y << ";" << val << std::endl;
  }
  return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_CBRT_HPP
#define STAN_MATH_PRIM_SCAL_FUN_CBRT_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/is_inf.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <boost/math/special_functions/cbrt.hpp>

namespace stan {
  namespace math {

    /**
     * Return the cube root of the specified value
     *
     * @param[in] x Argument.
     * @return Cube root of the argument.
     * @throw std::domain_error If argument is negative.
     */
    inline double cbrt(double x) {
      if (is_nan(x))
        return NOT_A_NUMBER;
      if (is_inf(x))
        return x < 0 ? NEGATIVE_INFTY : INFTY;
      return boost::math::cbrt(x, boost_policy_t());
    }

    /**
     * Integer version of cbrt.
     *
     * @param[in] x Argument.
     * @return Cube root of the argument.
     * @throw std::domain_error If argument is less than 1.
     */
    inline double cbrt(int x) {
      return cbrt(static_cast<double>(x));
    }

  }
}
#endif


#include <replay/planar_direction.hpp>
#include <boost/math/constants/constants.hpp>
#include <catch2/catch.hpp>

using namespace replay;
namespace
{
    constexpr auto pi = boost::math::constants::pi<float>();
    constexpr auto two_pi = boost::math::constants::two_pi<float>();
    constexpr auto half_pi = boost::math::constants::half_pi<float>();
}

TEST_CASE("can_move_around_singularities", "[planar_direction]")
{
    planar_direction current(-4.32833433f);
    planar_direction to(1.95485115f);

    auto next = planar_direction::move(current, to, 1000.f);
    REQUIRE(next == to);
}

TEST_CASE("can_move_toward_a_target", "[planar_direction]")
{
    planar_direction current(2.f);
    planar_direction to(1.f);

    auto next = planar_direction::move(current, to, 0.5f);
    REQUIRE(next.angle() == 1.5f);
}

TEST_CASE("move_picks_the_shorter_arc", "[planar_direction]")
{
    planar_direction current(0.1f);
    planar_direction to(two_pi - 0.1f);
    auto next = planar_direction::move(current, to, 0.1f);
    REQUIRE(next.angle() == 0.f);
}

TEST_CASE("normalizes_high_values_correctly", "[planar_direction]")
{
    REQUIRE(planar_direction(boost::math::constants::two_pi<float>()).normalized().angle() == Approx(0.f));
}

TEST_CASE("normalizes_low_values_correctly", "[planar_direction]")
{
    REQUIRE(planar_direction(-1.5f * pi).normalized().angle() == Approx(pi * 0.5f));
}

TEST_CASE("can_interpolate_between_positive_values", "[planar_direction]")
{
    REQUIRE(lerp(planar_direction(0.f), planar_direction(pi*2.5f), 0.25f).angle() == Approx(pi * 0.125f));
}


#include "plane.h"
#include <vector>
#include <math.h>
#include <Eigen/Dense>

#include <iostream>

using namespace Eigen;
using namespace std;

bool line3d::ClosestPoint(const line3d &L1, const line3d &L2, Vector3d &Result1, Vector3d &Result2)
{
	Vector3d NewDir = L1.D.cross(L2.D);
	float Length = NewDir.norm();
	if (Length == 0.0f) {
		return false;
	}

	plane P1 = plane::fromPointVectors(L1.P0, NewDir, L1.D);
	plane P2 = plane::fromPointVectors(L2.P0, NewDir, L2.D);

	Result1 = P2.IntersectLine(L1);
	Result2 = P1.IntersectLine(L2);
	return true;
}

float line3d::Dist(const line3d &L1, const line3d &L2)
{
	Vector3d Cross = L1.D.cross(L2.D);
	Vector3d v = L2.P0 - L1.P0;

	return abs(v.dot(Cross)) / Cross.norm();
}

float line3d::DistSq(const line3d &L1, const line3d &L2)
{
	Vector3d Cross = L1.D.cross(L2.D);

	Vector3d v = L2.P0 - L1.P0;
	float Dot = v.dot(Cross);
	return (Dot * Dot) / Cross.squaredNorm();
}

float line3d::DistToPoint(const Vector3d &P) const
{
	float t0 = D.dot(P - P0) / D.dot(D);

	Vector3d v = P0 + t0 * D;

	// TODO: Replace this with the eigen equivalent
	float x = P.x() + v.x();
	float y = P.y() + v.y();
	float z = P.z() + v.z();

	float distanceLine = sqrt(x*x + y*y + z*z);
	return distanceLine;
}

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

plane::plane()
{

}

float plane::get_x(float y, float z)
{
	return -(b*y + c*z + d) / a;
}

float plane::get_y(float x, float z)
{
	return -(a*x + c*z + d) / b;
}

float plane::get_z(float x, float y)
{
	return -(a*x + b*y + d) / c;
}

plane::plane(const plane &P)
{
	a = P.a;
	b = P.b;
	c = P.c;
	d = P.d;
}

plane::plane(float _a, float _b, float _c, float _d)
{
	a = _a;
	b = _b;
	c = _c;
	d = _d;
}

plane::plane(const Vector3d &NormalizedNormal, float _d)
{
	a = NormalizedNormal(0);
	b = NormalizedNormal(1);
	c = NormalizedNormal(2);
	d = _d;
}

plane plane::fromPointNormal(const Vector3d &Pt, const Vector3d &Normal)
{
	plane Result;
	Vector3d NormalizedNormal = Normal;
	NormalizedNormal.normalize();

	Result.a = NormalizedNormal(0);
	Result.b = NormalizedNormal(1);
	Result.c = NormalizedNormal(2);
	Result.d = -Pt.dot(NormalizedNormal);
	return Result;
}

plane plane::fromPointVectors(const Vector3d &Pt, const Vector3d &V1, const Vector3d &V2)
{
	Vector3d Normal = V1.cross(V2);
	return fromPointNormal(Pt, Normal);
}

plane plane::Normalize()
{
	plane Result;
	float Distance = sqrtf(a * a + b * b + c * c);
	Result.a = a / Distance;
	Result.b = b / Distance;
	Result.c = c / Distance;
	Result.d = d / Distance;
	return Result;
}

plane plane::fromPoints(const Vector3d &V0, const Vector3d &V1, const Vector3d &V2)
{
	Vector3d t0 = V1 - V0;
	Vector3d t1 = V2 - V0;
	Vector3d Normal = t0.cross(t1);
	Normal.normalize();
	return fromPointNormal(V0, Normal);
}

Vector3d plane::IntersectLine(const line3d &Line) const
{
	return IntersectLine(Line.P0, Line.P0 + Line.D);
}

Vector3d plane::IntersectLine(const Vector3d &V1, const Vector3d &V2) const
{
	Vector3d Diff = V1 - V2;
	float Denominator = a * Diff(0) + b * Diff(1) + c * Diff(2);
	if (Denominator == 0.0f) {
		return (V1 + V2) * 0.5f;
	}
	float u = (a * V1(0) + b * V1(1) + c * V1(2) + d) / Denominator;

	return (V1 + u * (V2 - V1));
}

Vector3d plane::IntersectLine(const Vector3d &V1, const Vector3d &V2, bool &Hit) const
{
	Hit = true;
	Vector3d Diff = V2 - V1;
	float denominator = a * Diff(0) + b * Diff(1) + c * Diff(2);
	if (denominator == 0) { Hit = false; return V1; }
	float u = (a * V1(0) + b * V1(1) + c * V1(2) + d) / denominator;

	return (V1 + u * (V2 - V1));
}

float plane::IntersectLineRatio(const Vector3d &V1, const Vector3d &V2)
{
	Vector3d Diff = V2 - V1;
	float Denominator = a * Diff(0) + b * Diff(1) + c * Diff(2);
	if (Denominator == 0.0f) {
		return 0.0f;
	}
	return (a * V1(0) + b * V1(1) + c * V1(2) + d) / -Denominator;
}

float plane::signedDistance(const Vector3d &Pt) const
{
	return (a * Pt(0) + b * Pt(1) + c * Pt(2) + d);
}

float plane::unsignedDistance(const Vector3d &Pt) const
{
	return abs(a * Pt(0) + b * Pt(1) + c * Pt(2) + d);
}

Vector3d plane::ClosestPoint(const Vector3d &Point)
{
	return (Point - Normal() * signedDistance(Point));
}

bool plane::planePlaneIntersection(const plane &P1, const plane &P2, line3d &L)
{
	float Denominator = P1.a * P2.b - P1.b * P2.a;
	if (Denominator == 0.0f) {
		// this case should be handled by switching axes...
		return false;
	}
	L.P0 = Vector3d((P2.d * P1.b - P1.d * P2.b) /
		Denominator, (P1.d * P2.a - P2.d * P1.a) /
		Denominator, 0.0f);

	L.D = P1.Normal().cross(P2.Normal());
	if (L.D.norm() == 0.0f) {
		return false;
	}
	L.D.normalize();

	return true;
}

float plane::Dot(const plane &P, const Vector4d &V)
{
	return P.a * V(0) + P.b * V(1) + P.c * V(2) + P.d * V(3);
}

float plane::DotCoord(const plane &P, const Vector3d &V)
{
	return P.a * V(0) + P.b * V(1) + P.c * V(2) + P.d;
}

float plane::DotNormal(const plane &P, const Vector3d &V)
{
	return P.a * V(0) + P.b * V(1) + P.c * V(2);
}


#ifndef OBJECT_DETECTION_MSGS_NON_MAXIMUM_SUPPRESSOR_HPP
#define OBJECT_DETECTION_MSGS_NON_MAXIMUM_SUPPRESSOR_HPP

#include <functional> // for std::greater<>
#include <iterator>   // for std::advance()
#include <map>
#include <string>
#include <utility> // for std::make_pair()
#include <vector>

#include <nodelet/nodelet.h>
#include <object_detection_msgs/Objects.h>
#include <object_detection_msgs/Points.h>
#include <object_detection_msgs/cv_conversions.hpp>
#include <ros/node_handle.h>
#include <ros/publisher.h>
#include <ros/subscriber.h>

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

#include <boost/foreach.hpp>

namespace object_detection_msgs {

// calc overlap of 2 contours
static inline double computeOverlap(const std::vector<cv::Point> &a,
                                    const std::vector<cv::Point> &b) {
  // rectangle of interest
  const cv::Rect roi(cv::boundingRect(a) & cv::boundingRect(b));
  const cv::Point offset(-roi.tl());

  // draw a polygon represented by contour A
  cv::Mat poly_a(cv::Mat::zeros(roi.size(), CV_8UC1));
  cv::fillPoly(poly_a, std::vector<std::vector<cv::Point>>(1, a), 1, cv::LINE_8, 1, offset);

  // draw a polygon represented by contour B
  cv::Mat poly_b(cv::Mat::zeros(roi.size(), CV_8UC1));
  cv::fillPoly(poly_b, std::vector<std::vector<cv::Point>>(1, b), 1, cv::LINE_8, 1, offset);

  // calc <area of polygon A and B> / <area of polygon A or B>
  //    (0: no overlap, 1: complete overlap)
  return static_cast<double>(cv::countNonZero(poly_a & poly_b)) / cv::countNonZero(poly_a | poly_b);
}

// variant of cv::dnn::NMSBoxes() for general contours
static inline void NMSContours(const std::vector<std::vector<cv::Point>> &contours,
                               const std::vector<double> &scores, const double score_threshold,
                               const double nms_threshold, std::vector<int> &indices,
                               const double eta = 1., const int top_k = 0) {
  // sort scores (with corresponding indices)
  typedef std::multimap<double, int, std::greater<double>> ScoreMap;
  ScoreMap score_map;
  for (std::size_t i = 0; i < std::min(scores.size(), contours.size()); ++i) {
    // validate score
    const double score(scores[i]);
    if (score < 0. || score < score_threshold || score > 1.) {
      continue;
    }
    // validate contour associated to score
    if (contours[i].empty()) {
      continue;
    }
    score_map.insert(std::make_pair(score, i));
  }

  // keep top_k scores if needed.
  if (top_k > 0 && top_k < score_map.size()) {
    ScoreMap::iterator erase_begin(score_map.begin());
    std::advance(erase_begin, top_k);
    score_map.erase(erase_begin, score_map.end());
  }

  // do nms.
  double adaptive_threshold(nms_threshold);
  indices.clear();
  BOOST_FOREACH (const ScoreMap::value_type &score_pair, score_map) {
    const int idx(score_pair.second);
    bool keep(true);
    BOOST_FOREACH (const int kept_idx, indices) {
      if (computeOverlap(contours[idx], contours[kept_idx]) > adaptive_threshold) {
        keep = false;
        break;
      }
    }
    if (keep) {
      indices.push_back(idx);
      if (eta < 1. && adaptive_threshold > 0.5) {
        adaptive_threshold *= eta;
      }
    }
  }
}

class NonMaximumSuppressor : public nodelet::Nodelet {
public:
  NonMaximumSuppressor() {}
  virtual ~NonMaximumSuppressor() {}

private:
  virtual void onInit() {
    ros::NodeHandle &nh(getNodeHandle());
    ros::NodeHandle &pnh(getPrivateNodeHandle());

    score_threshold_ = pnh.param("score_threshold", 0.4);
    nms_threshold_ = pnh.param("nms_threshold", 0.5);
    eta_ = pnh.param("eta", 1.);
    top_k_ = pnh.param("top_k", 0);

    publisher_ = nh.advertise<Objects>("objects_out", 1, true);
    subscriber_ = nh.subscribe("objects_in", 1, &NonMaximumSuppressor::suppress, this);
  }

  void suppress(const ObjectsConstPtr &object_in) {
    // non maximum suppression
    std::vector<int> indices;
    NMSContours(toCvContours(object_in->contours), object_in->probabilities, score_threshold_,
                nms_threshold_, indices, eta_, top_k_);

    // pick kept objects
    const ObjectsPtr object_out(new Objects);
    object_out->header.stamp = object_in->header.stamp;
    BOOST_FOREACH (const int idx, indices) {
      object_out->names.push_back(idx < object_in->names.size() ? object_in->names[idx]
                                                                : std::string(""));
      object_out->contours.push_back(idx < object_in->contours.size() ? object_in->contours[idx]
                                                                      : Points());
      object_out->probabilities.push_back(
          idx < object_in->probabilities.size() ? object_in->probabilities[idx] : -1.);
    }

    // publish kept objects
    publisher_.publish(object_out);
  }

private:
  double score_threshold_, nms_threshold_, eta_;
  int top_k_;

  ros::Publisher publisher_;
  ros::Subscriber subscriber_;
};
} // namespace object_detection_msgs

#endif


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/protect.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/list.hpp>

#include <cmath>
#include <boost/safe_float.hpp>
#include <boost/safe_float/convenience.hpp>
#include <boost/safe_float/policy/check_subtraction_overflow.hpp>
#include <boost/safe_float/policy/check_subtraction_underflow.hpp>
#include <boost/safe_float/policy/check_subtraction_inexact.hpp>
#include <boost/safe_float/policy/check_subtraction_invalid_result.hpp>

//types to be tested
using test_types=boost::mpl::list<
    float, double, long double
>;

using namespace boost::safe_float;

/**
  This test suite checks different policies on subtraction operations using default parameters for the other policies.
  */
BOOST_AUTO_TEST_SUITE( safe_float_subtraction_test_suite )

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_subtraction_throws_on_overflow, FPT, test_types){
    // define two FPT numbers suppose to positive overflow
    FPT a = std::numeric_limits<FPT>::max();
    FPT b = std::numeric_limits<FPT>::lowest();
    // check FPT overflows to inf after subtract
    BOOST_CHECK(std::isinf(a-b));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_subtraction_overflow> c(std::numeric_limits<FPT>::max());
    safe_float<FPT, policy::check_subtraction_overflow> d(std::numeric_limits<FPT>::lowest());

    // check the subtraction throws
    BOOST_CHECK_THROW(c-d, std::exception);

    // define two FPT numbers suppose to negative overflow
    FPT e = std::numeric_limits<FPT>::lowest();
    FPT f = std::numeric_limits<FPT>::max();
    // check FPT overflows to inf after add
    BOOST_CHECK(std::isinf(e-f));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_subtraction_overflow> g(std::numeric_limits<FPT>::lowest());
    safe_float<FPT, policy::check_subtraction_overflow> h(std::numeric_limits<FPT>::max());

    // check the subtraction throws
    BOOST_CHECK_THROW(g-h, std::exception);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_subtraction_inexact_rounding, FPT, test_types){
    // define two FPT numbers suppose to round the result inexactly
    FPT a = std::numeric_limits<FPT>::min();
    FPT b = pow(2, std::numeric_limits<FPT>::digits);

    // check substracting a from b is a new value.
    BOOST_CHECK(b-a == b);

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_subtraction_inexact> c(std::numeric_limits<FPT>::min());
    safe_float<FPT, policy::check_subtraction_inexact> d((FPT)pow(2, std::numeric_limits<FPT>::digits));

    // check the subtraction throws
    BOOST_CHECK_THROW(d-c, std::exception);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_subtraction_underflow, FPT, test_types){
    // define two FPT numbers suppose to underflow
    FPT a, b, c;
    if (std::is_same<FPT, float>()) {
        a = 4.01254977e-38f;
        b = 4.01254949e-38f;

        BOOST_CHECK(std::isnormal(a));
        BOOST_CHECK(std::isnormal(b));

        //check the subtraction produces an denormal result (considered underflow)
        c = a - b;
        BOOST_CHECK( std::fpclassify( c ) == FP_SUBNORMAL ) ;

        // construct safe_float version of the same two numbers
        safe_float<FPT, policy::check_subtraction_underflow> d(a);
        safe_float<FPT, policy::check_subtraction_underflow> e(b);

        // check the subtraction throws
        BOOST_CHECK_THROW(d-e, std::exception);
    } else if (std::is_same<FPT, double>()) {
        a =  2.2250738585072019e-308;
        b =  2.2250738585072014e-308;

        BOOST_CHECK(std::isnormal(a));
        BOOST_CHECK(std::isnormal(b));

        //check the subtraction produces an denormal result (considered underflow)
        c = a - b;
        BOOST_CHECK( std::fpclassify( c ) == FP_SUBNORMAL ) ;

        // construct safe_float version of the same two numbers
        safe_float<FPT, policy::check_subtraction_underflow> d(a);
        safe_float<FPT, policy::check_subtraction_underflow> e(b);

        // check the subtraction throws
        BOOST_CHECK_THROW(d-e, std::exception);
    } else if (std::is_same<FPT, long double>()) {
        a = 3.40132972460942461217e-4932l;
        b = 3.40132972460942461181e-4932l;

        BOOST_CHECK(std::isnormal(a));
        BOOST_CHECK(std::isnormal(b));

        //check the subtraction produces an denormal result (considered underflow)
        c = a - b;
        BOOST_CHECK( std::fpclassify( c ) == FP_SUBNORMAL ) ;

        // construct safe_float version of the same two numbers
        safe_float<FPT, policy::check_subtraction_underflow> d(a);
        safe_float<FPT, policy::check_subtraction_underflow> e(b);

        // check the subtraction throws
        BOOST_CHECK_THROW(d-e, std::exception);
    } else {
        BOOST_ERROR("underflow test only implemented for double so far");
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE( safe_float_subtraction_invalid_result, FPT, test_types){
    // define two FPT numbers suppose to produce a NAN
    FPT a = std::numeric_limits<FPT>::infinity();
    FPT b = std::numeric_limits<FPT>::infinity();

    // check adding produced NaN
    BOOST_CHECK(std::isnan(a-b));

    // construct safe_float version of the same two numbers
    safe_float<FPT, policy::check_subtraction_invalid_result> c(std::numeric_limits<FPT>::infinity());
    safe_float<FPT, policy::check_subtraction_invalid_result> d((std::numeric_limits<FPT>::infinity()));

    // check the subtraction throws
    BOOST_CHECK_THROW(c-d, std::exception);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file mtgp64-calc-poly.cpp
 *
 * @brief calculate characteristic polynomial for 64bit mtgp.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (Hiroshima University)
 *
 * Copyright (c) 2012 Mutsuo Saito, Makoto Matsumoto, Hiroshima
 * University and University of Tokyo. All rights reserved.
 *
 * The new BSD License is applied to this software, see LICENSE.txt
 */
#include <stdint.h>
#include <inttypes.h>
#include <time.h>
#include <string.h>
#include <string>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>
#include "mtgp64-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp64-fast.h"

using namespace std;
using namespace NTL;

/**
 * calculate the characteristic polynomial for given 64-bit MTGP.
 * MinPolySeq is defined in NTL.
 * @param[out] poly calculated characteristic polynomial.
 * @param[in] mtgp64 generator
 */
void mtgp64_calc_characteristic(GF2X& poly, mtgp64_fast_t * mtgp64)
{
    vec_GF2 seq;
    int mexp = mtgp64->params.mexp;
    seq.SetLength(2 * mexp);
    for (int i = 0; i < 2 * mexp; i++) {
	seq[i] = mtgp64_genrand_uint64(mtgp64) & 1;
    }
    MinPolySeq(poly, seq, mexp);
}

/**
 * calculate the characteristic polynomial for given 64-bit MTGP.
 * MinPolySeq is defined in NTL.
 * @param[out] str calculated characteristic polynomial in string format.
 * @param[in] mtgp64 generator
 */
void mtgp64_calc_characteristic(string& str, mtgp64_fast_t * mtgp64)
{
    GF2X poly;
    mtgp64_calc_characteristic(poly, mtgp64);
    polytostring(str, poly);
}

#if defined(MAIN)
/**
 * main function for executable.
 * @param[in] argc number of arguments.
 * @param[in] argv an array of arguments.
 * @return 0 if normal, other abnormal.
 */
int main(int argc, char *argv[]) {
    int mexp;
    int no;
    uint32_t seed = 1;
    mtgp64_params_fast_t *params;
    mtgp64_fast_t mtgp64;
    int rc;

    if (argc <= 2) {
	printf("%s: mexp no.\n", argv[0]);
	return 1;
    }
    mexp = strtol(argv[1], NULL, 10);
    if (errno) {
	printf("%s: mexp no.\n", argv[0]);
	return 2;
    }
    no = strtol(argv[2], NULL, 10);
    if (errno) {
	printf("%s: mexp no.\n", argv[0]);
	return 3;
    }
    switch (mexp) {
    case 23209:
	params = mtgp64_params_fast_23209;
	break;
    case 44497:
	params = mtgp64_params_fast_44497;
	break;
    case 110503:
	params = mtgp64_params_fast_110503;
	break;
    default:
	printf("%s: mexp no.\n", argv[0]);
	printf("mexp shuould be 23209, 44497 or 110503\n");
	return 4;
    }
    if (no >= 128 || no < 0) {
	printf("%s: mexp no.\n", argv[0]);
	printf("no must be between 0 and 127\n");
	return 5;
    }
    params += no;
    rc = mtgp64_init(&mtgp64, params, seed);
    if (rc) {
	printf("failure in mtgp64_init\n");
	return -1;
    }
    mtgp64_print_idstring(&mtgp64, stdout);
    string s;
    mtgp64_calc_characteristic(s, &mtgp64);
    printf("%s\n", s.c_str());
    mtgp64_free(&mtgp64);
    return 0;
}
#endif


//
// Copyright (c) Leonid Seniukov. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for details.
//

#include "relativePoseEstimators/Estimator3Points.h"

#include <Eigen/Eigen>

namespace gdr {

    SE3 Estimator3Points::getRt(const Eigen::Matrix4Xd &toBeTransformed3Points,
                                const Eigen::Matrix4Xd &dest3Points,
                                const CameraRGBD &cameraIntrToBeTransformed,
                                const CameraRGBD &cameraIntrDestination) const {
        int dim = 3;
        int minNumPoints = 3;
        int numPoints = toBeTransformed3Points.cols();
        assert(numPoints == minNumPoints);
        assert(numPoints == dest3Points.cols());

        Eigen::Matrix4d umeyama3p = umeyama(toBeTransformed3Points.block(0, 0, dim, dim),
                                            dest3Points.block(0, 0, dim, dim));
        return SE3(umeyama3p);
    }
}


#include <math.h>
#include <stdio.h>

#include <boost/math/quaternion.hpp>

static double inertia_z = 6.55929728e-05;
static double frequency =3.0;

static double stiffness= inertia_z * pow(2*M_PI*frequency,2);
static double angle_init = -0.2300258882727588;
//static double angle_init = -0.8;

extern "C" void externalForces(double t, double *f, unsigned int size_z, double *z)
{
  f[0]=0;
  f[1]=0;
  f[2]=-10;
}
extern "C" void externalMoment(double t,double *m, unsigned int size_z, double *z)
{
  m[0]=0;
  m[1]=0;
  m[2]=0;
}

extern "C" void internalForces(double t, double *q, double *v, double *f, unsigned int size_z,double *z)
{
  // Simple spring in z direction.
  f[0]=0;
  f[1]=0;
  f[2]=0.;
  // printf("internalForcesB1 :\n");
  // printf("f[0] = %e\t f[1] = %e\t, f[2]=%e\n",f[0],f[1],f[2]);
}

extern "C" void internalForcesB1_Jacq(double t, double *q, double *v, double *jac, unsigned int size_z,double *z)
{
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j<7; j++)
      jac[i+j*3]=0.0;
  }
  jac[2+2*3]=1e4;
  // printf("internalForcesB1_Jacq :\n");
  // printf("jac[2+2*3] = %e\n", jac[2+2*3]);
}

extern "C" void internalMomentsBalanceWheel(double t, double *q, double *v, double *m, unsigned int size_z,double *z)
{
  //  printf("internalMomentsB1 :\n");
  // Simple torsional spring around z axis
  // printf("q[3] = %e\n", q[3]);
  // printf("q[4] = %e\n", q[4]);
  // printf("q[5] = %e\n", q[5]);
  // printf("q[6] = %e\n", q[6]);

  double angle = 2*asin(q[6]);
  //printf("angle = %e\n", angle);
  //printf("stiffness = %e \n", stiffness);
  m[0]=0.0;
  m[1]=0.;
  m[2]=stiffness * (angle-angle_init);
  //printf("m[0] = %e\t m[1] = %e\t, m[2]=%e\n",m[0],m[1],m[2]);
}

extern "C" void internalMomentsBalanceWheel_Jacq(double t, double *q, double *v, double *jac, unsigned int size_z,double *z)
{
  //printf("internalMomentsB1_Jacq :\n");
  for(int i =0; i < 3; i++)
  {
    for(int j=0; j<7; j++)
      jac[i+j*3]=0.0;
  }
  // printf("q[3] = %e\n", q[3]);
  // printf("q[4] = %e\n", q[4]);
  // printf("q[5] = %e\n", q[5]);
  // printf("q[6] = %e\n", q[6]);

  //double angle = 2*asin(q[6]);
  // printf("angle = %e\n", angle);
  jac[2+6*3]=stiffness * 2.0 / sqrt(1 - q[6]*q[6]) ;
  // printf("jac[3+3*3] = %e\n", jac[3+3*3]);
}

extern "C" void externalMomentEscapeWheel(double t,double *m, unsigned int size_z, double *z)
{
  m[0]=0;
  m[1]=0;
  m[2]=-0.001;
}

extern "C" void prescribedvelocityB1(double time, unsigned int sizeofprescribedvelocity, double *pv)
{
  /* the plugin implements v(t) = C + A cos(omega *t) */

  double C = -150.0 ;
  // double omega = M_PI / 2.0;
  // double A = 10.0;

  //pv[0] =  A * cos(omega * time*100.0);
  pv[0] =  C;
  //printf("prescribed velocity = %e\n", pv[0]);
}


#ifndef STAN_MATH_FWD_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP
#define STAN_MATH_FWD_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP

#include <stan/math/fwd/meta.hpp>
#include <stan/math/fwd/core.hpp>

#include <boost/math/special_functions/digamma.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>

namespace stan {
namespace math {

template <typename T>
inline fvar<T> binomial_coefficient_log(const fvar<T>& x1, const fvar<T>& x2) {
  using boost::math::digamma;
  using std::log;
  const double cutoff = 1000;
  if ((x1.val_ < cutoff) || (x1.val_ - x2.val_ < cutoff)) {
    return fvar<T>(binomial_coefficient_log(x1.val_, x2.val_),
                   x1.d_ * digamma(x1.val_ + 1) - x2.d_ * digamma(x2.val_ + 1)
                       - (x1.d_ - x2.d_) * digamma(x1.val_ - x2.val_ + 1));
  } else {
    return fvar<T>(
        binomial_coefficient_log(x1.val_, x2.val_),
        x2.d_ * log(x1.val_ - x2.val_)
            + x2.val_ * (x1.d_ - x2.d_) / (x1.val_ - x2.val_)
            + x1.d_ * log(x1.val_ / (x1.val_ - x2.val_))
            + (x1.val_ + 0.5) / (x1.val_ / (x1.val_ - x2.val_))
                  * (x1.d_ * (x1.val_ - x2.val_) - (x1.d_ - x2.d_) * x1.val_)
                  / ((x1.val_ - x2.val_) * (x1.val_ - x2.val_))
            - x1.d_ / (12.0 * x1.val_ * x1.val_) - x2.d_
            + (x1.d_ - x2.d_)
                  / (12.0 * (x1.val_ - x2.val_) * (x1.val_ - x2.val_))
            - digamma(x2.val_ + 1) * x2.d_);
  }
}

template <typename T>
inline fvar<T> binomial_coefficient_log(const fvar<T>& x1, double x2) {
  using boost::math::digamma;
  using std::log;
  const double cutoff = 1000;
  if ((x1.val_ < cutoff) || (x1.val_ - x2 < cutoff)) {
    return fvar<T>(
        binomial_coefficient_log(x1.val_, x2),
        x1.d_ * digamma(x1.val_ + 1) - x1.d_ * digamma(x1.val_ - x2 + 1));
  } else {
    return fvar<T>(binomial_coefficient_log(x1.val_, x2),
                   x2 * x1.d_ / (x1.val_ - x2)
                       + x1.d_ * log(x1.val_ / (x1.val_ - x2))
                       + (x1.val_ + 0.5) / (x1.val_ / (x1.val_ - x2))
                             * (x1.d_ * (x1.val_ - x2) - x1.d_ * x1.val_)
                             / ((x1.val_ - x2) * (x1.val_ - x2))
                       - x1.d_ / (12.0 * x1.val_ * x1.val_)
                       + x1.d_ / (12.0 * (x1.val_ - x2) * (x1.val_ - x2)));
  }
}

template <typename T>
inline fvar<T> binomial_coefficient_log(double x1, const fvar<T>& x2) {
  using boost::math::digamma;
  using std::log;
  const double cutoff = 1000;
  if ((x1 < cutoff) || (x1 - x2.val_ < cutoff)) {
    return fvar<T>(
        binomial_coefficient_log(x1, x2.val_),
        -x2.d_ * digamma(x2.val_ + 1) - x2.d_ * digamma(x1 - x2.val_ + 1));
  } else {
    return fvar<T>(binomial_coefficient_log(x1, x2.val_),
                   x2.d_ * log(x1 - x2.val_) + x2.val_ * -x2.d_ / (x1 - x2.val_)
                       - x2.d_
                       - x2.d_ / (12.0 * (x1 - x2.val_) * (x1 - x2.val_))
                       + x2.d_ * (x1 + 0.5) / (x1 - x2.val_)
                       - digamma(x2.val_ + 1) * x2.d_);
  }
}
}  // namespace math
}  // namespace stan
#endif


#include "STIPNode.h"

#include <Eigen/Core>

#include <boost/timer.hpp>

#include <iostream>
#include <numeric>

namespace nuisken {
namespace randomforests {

STIPNode::MeasureType STIPNode::decideType() {
    std::uniform_int_distribution<> distribution(0, 1);
    int typeNumber = distribution(RandomGenerator::getInstance().generator_);
    switch (typeNumber) {
        case 0:
            return CLASS;
        case 1:
            return VECTOR;
        default:
            return CLASS;
    }
}

STIPSplitParameters STIPNode::generateRandomParameter() {
    std::uniform_int_distribution<> channelDistribution(0, numberOfFeatureChannels - 1);
    int featureChannel = channelDistribution(RandomGenerator::getInstance().generator_);

    std::uniform_int_distribution<> indexDistribution(
            0, numberOfFeatureDimensions.at(featureChannel) - 1);
    int index1 = indexDistribution(RandomGenerator::getInstance().generator_);
    // int index2;
    // do {
    int index2 = indexDistribution(RandomGenerator::getInstance().generator_);
    //} while (index1 == index2);

    return STIPSplitParameters(index1, index2, featureChannel);
}

double STIPNode::evaluateSplit(const std::vector<FeatureRawPtr>& leftFeatures,
                               const std::vector<FeatureRawPtr>& rightFeatures) const {
    auto leftValue = 0.0;
    auto rightValue = 0.0;

    switch (type) {
        case CLASS:
            leftValue = calculateClassUncertainty(leftFeatures);
            rightValue = calculateClassUncertainty(rightFeatures);
            break;
        case VECTOR:
            leftValue = calculateVectorUncertainty(leftFeatures);
            rightValue = calculateVectorUncertainty(rightFeatures);
            break;
    }

    return (leftValue + rightValue) / (leftFeatures.size() + rightFeatures.size());
}

double STIPNode::calculateClassUncertainty(const std::vector<FeatureRawPtr>& features) const {
    //各クラスの割合を計算
    Eigen::VectorXd classProbabilities = Eigen::VectorXd::Zero(numberOfClasses);
    auto oneProbability = 1.0 / features.size();
    for (const auto& feature : features) {
        classProbabilities(feature->getClassLabel()) += oneProbability;
    }

    //曖昧さ（エントロピー）を計算
    double uncertainty = 0.0;
    for (auto i = 0; i < classProbabilities.size(); ++i) {
        if (0.0 != classProbabilities(i)) {
            uncertainty += classProbabilities(i) * std::log(classProbabilities(i));
        }
    }
    uncertainty *= static_cast<double>(features.size());

    return uncertainty;
}

double STIPNode::calculateVectorUncertainty(const std::vector<FeatureRawPtr>& features) const {
    // displacementVectorの平均を計算
    std::vector<cv::Vec3f> meanDisplacementVectors(numberOfClasses);
    Eigen::VectorXi sizes = Eigen::VectorXi::Zero(numberOfClasses);

    auto end = std::end(features);
    for (auto itr = std::begin(features); itr != end; ++itr) {
        auto displacementVector = (*itr)->getDisplacementVector();
        auto classLabel = (*itr)->getClassLabel();

        meanDisplacementVectors.at(classLabel) += displacementVector;

        ++sizes(classLabel);
    }
    for (auto i = 0; i < numberOfClasses; ++i) {
        meanDisplacementVectors.at(i) /= static_cast<double>(sizes(i));
    }

    //曖昧さを計算
    auto uncertainty = 0.0;
    for (auto itr = std::begin(features); itr != end; ++itr) {
        // cv::Vec3iをcv::Vec3fに変換
        cv::Vec3f displacementVector((*itr)->getDisplacementVector());
        auto difference = displacementVector - meanDisplacementVectors.at((*itr)->getClassLabel());

        uncertainty += cv::norm(difference);
    }

    return -uncertainty;
}

bool STIPNode::decision(const FeatureRawPtr& feature, const STIPSplitParameters& splitParameter,
                        double tau) const {
    double value1 = feature->getFeatureValue(splitParameter.getIndex1(),
                                             splitParameter.getFeatureChannel());
    double value2 = feature->getFeatureValue(splitParameter.getIndex2(),
                                             splitParameter.getFeatureChannel());

    if (value1 < (value2 + tau)) {
        return true;
    } else {
        return false;
    }
}

std::shared_ptr<STIPLeaf> STIPNode::calculateLeafData(
        const std::vector<FeatureRawPtr>& features) const {
    std::vector<STIPLeaf::FeatureInfo> featureInfo;

    auto end = std::end(features);
    for (auto itr = std::begin(features); itr != end; ++itr) {
        featureInfo.emplace_back((*itr)->getIndex(), (*itr)->getClassLabel(),
                                 (*itr)->getSpatialScale(), (*itr)->getTemporalScale(),
                                 (*itr)->getDisplacementVector());
    }

    return std::make_shared<STIPLeaf>(featureInfo);
}

std::shared_ptr<STIPLeaf> STIPNode::loadLeafData(std::queue<std::string>& nodeElements) const {
    auto leaf = std::make_shared<STIPLeaf>();
    leaf->load(nodeElements);

    return leaf;
}
}
}


/**
 * @file cv_test.cpp
 *
 * Unit tests for the cross-validation module.
 *
 * 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 <type_traits>

#include <mlpack/core/cv/meta_info_extractor.hpp>
#include <mlpack/core/cv/metrics/accuracy.hpp>
#include <mlpack/core/cv/metrics/f1.hpp>
#include <mlpack/core/cv/metrics/mse.hpp>
#include <mlpack/core/cv/metrics/precision.hpp>
#include <mlpack/core/cv/metrics/recall.hpp>
#include <mlpack/core/cv/simple_cv.hpp>
#include <mlpack/core/cv/k_fold_cv.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/const_init.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/decision_tree/decision_tree.hpp>
#include <mlpack/methods/decision_tree/information_gain.hpp>
#include <mlpack/methods/hoeffding_trees/hoeffding_tree.hpp>
#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>
#include <mlpack/methods/naive_bayes/naive_bayes_classifier.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression.hpp>
#include <mlpack/core/data/confusion_matrix.hpp>
#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "mock_categorical_data.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::cv;
using namespace mlpack::naive_bayes;
using namespace mlpack::regression;
using namespace mlpack::tree;
using namespace mlpack::data;

BOOST_AUTO_TEST_SUITE(CVTest);

/**
 * Test metrics for binary classification.
 */
BOOST_AUTO_TEST_CASE(BinaryClassificationMetricsTest)
{
  // Using the same data for training and testing.
  arma::mat data = arma::linspace<arma::rowvec>(1.0, 10.0, 10);

  // Labels that will be considered as "ground truth".
  arma::Row<size_t> labels("0 0 1 0 0  1 0 1 0 1");

  // Labels that make the data linearly separable. These labels will be
  // predicted in response to the data since we use them for training.
  arma::Row<size_t> predictedLabels("0 0 0 0 0  1 1 1 1 1");

  LogisticRegression<> lr(data, predictedLabels);

  BOOST_REQUIRE_CLOSE(Accuracy::Evaluate(lr, data, labels), 0.7, 1e-5);

  BOOST_REQUIRE_CLOSE(Precision<Binary>::Evaluate(lr, data, labels), 0.6, 1e-5);

  BOOST_REQUIRE_CLOSE(Recall<Binary>::Evaluate(lr, data, labels), 0.75, 1e-5);

  double f1 = 2 * 0.6 * 0.75 / (0.6 + 0.75);
  BOOST_REQUIRE_CLOSE(F1<Binary>::Evaluate(lr, data, labels), f1, 1e-5);
}

/**
 * Test for confusion matrix.
 */
BOOST_AUTO_TEST_CASE(ConfusionMatrixTest)
{
  // Labels that will be considered as "ground truth".
  arma::Row<size_t> labels("0 0 1 0 0  1 0 1 0 1");

  // Predicted labels.
  arma::Row<size_t> predictedLabels("0 0 0 0 0  1 1 1 1 1");
  // Confusion matrix.
  arma::Mat<int> output;
  data::ConfusionMatrix(predictedLabels, labels, output, 2);
  BOOST_REQUIRE_EQUAL(output(0, 0), 4);
  BOOST_REQUIRE_EQUAL(output(0, 1), 1);
  BOOST_REQUIRE_EQUAL(output(1, 0), 2);
  BOOST_REQUIRE_EQUAL(output(1, 1), 3);
}

/**
 * Test metrics for multiclass classification.
 */
BOOST_AUTO_TEST_CASE(MulticlassClassificationMetricsTest)
{
  // Using the same data for training and testing.
  arma::mat data = arma::linspace<arma::rowvec>(1.0, 12.0, 12);

  // Labels that will be considered as "ground truth".
  arma::Row<size_t> labels("0 1  0 1  2 2 1 2  3 3 3 3");

  // These labels should be predicted in response to the data since we use them
  // for training.
  arma::Row<size_t> predictedLabels("0 0  1 1  2 2 2 2  3 3 3 3");
  size_t numClasses = 4;

  NaiveBayesClassifier<> nb(data, predictedLabels, numClasses);

  // Assert that the Naive Bayes model really predicts the labels above in
  // response to the data.
  BOOST_REQUIRE_CLOSE(Accuracy::Evaluate(nb, data, predictedLabels), 1.0, 1e-5);

  double microaveragedPrecision = double(1 + 1 + 3 + 4) / 12;
  BOOST_REQUIRE_CLOSE(Precision<Micro>::Evaluate(nb, data, labels),
      microaveragedPrecision, 1e-5);

  double microaveragedRecall = double(1 + 1 + 3 + 4) / 12;
  BOOST_REQUIRE_CLOSE(Recall<Micro>::Evaluate(nb, data, labels),
      microaveragedRecall, 1e-5);

  double microaveragedF1 = 2 * microaveragedPrecision * microaveragedRecall /
    (microaveragedPrecision + microaveragedRecall);
  BOOST_REQUIRE_CLOSE(F1<Micro>::Evaluate(nb, data, labels),
      microaveragedF1, 1e-5);

  double macroaveragedPrecision = (0.5 + 0.5 + 0.75 + 1.0) / 4;
  BOOST_REQUIRE_CLOSE(Precision<Macro>::Evaluate(nb, data, labels),
      macroaveragedPrecision, 1e-5);

  double macroaveragedRecall = (0.5 + 1.0 / 3 + 1.0 + 1.0) / 4;
  BOOST_REQUIRE_CLOSE(Recall<Macro>::Evaluate(nb, data, labels),
      macroaveragedRecall, 1e-5);

  double macroaveragedF1 = (2 * 0.5 * 0.5 / (0.5 + 0.5) +
      2 * 0.5 * (1.0 / 3) / (0.5 + (1.0 / 3)) + 2 * 0.75 * 1.0 / (0.75 + 1.0) +
      2 * 1.0 * 1.0 / (1.0 + 1.0)) / 4;
  BOOST_REQUIRE_CLOSE(F1<Macro>::Evaluate(nb, data, labels),
      macroaveragedF1, 1e-5);
}

/**
 * Test the mean squared error.
 */
BOOST_AUTO_TEST_CASE(MSETest)
{
  // Making two points that define the linear function f(x) = x - 1
  arma::mat trainingData("0 1");
  arma::rowvec trainingResponses("-1 0");

  LinearRegression lr(trainingData, trainingResponses);

  // Making three responses that differ from the correct ones by 0, 1, and 2
  // respectively
  arma::mat data("2 3 4");
  arma::rowvec responses("1 3 5");

  double expectedMSE = (0 * 0 + 1 * 1 + 2 * 2) / 3.0;

  BOOST_REQUIRE_CLOSE(MSE::Evaluate(lr, data, responses), expectedMSE, 1e-5);
}

/**
 * Test the mean squared error with matrix responses.
 */
BOOST_AUTO_TEST_CASE(MSEMatResponsesTest)
{
  arma::mat data("1 2");
  arma::mat trainingResponses("1 2; 3 4");

  FFN<MeanSquaredError<>, ConstInitialization> ffn(MeanSquaredError<>(),
    ConstInitialization(0));
  ffn.Add<Linear<>>(1, 2);
  ffn.Add<IdentityLayer<>>();

  ens::RMSProp opt(0.2);
  opt.BatchSize() = 1;
  opt.Shuffle() = false;
  ffn.Train(data, trainingResponses, opt);

  // Making four responses that differ from the correct ones by 0, 1, 2 and 3
  // respectively
  arma::mat responses("1 3; 5 7");

  double expectedMSE = (0 * 0 + 1 * 1 + 2 * 2 + 3 * 3) / 4.0;

  BOOST_REQUIRE_CLOSE(MSE::Evaluate(ffn, data, responses), expectedMSE, 1e-1);
}

template<typename Class,
         typename ExpectedPT,
         typename PassedMT = arma::mat,
         typename PassedPT = arma::Row<size_t>>
void CheckPredictionsType()
{
  using Extractor = MetaInfoExtractor<Class, PassedMT, PassedPT>;
  using ActualPT = typename Extractor::PredictionsType;
  static_assert(std::is_same<ExpectedPT, ActualPT>::value,
      "Should be the same");
}

/**
 * Test MetaInfoExtractor correctly recognizes the type of predictions for a
 * given machine learning algorithm.
 */
BOOST_AUTO_TEST_CASE(PredictionsTypeTest)
{
  CheckPredictionsType<LinearRegression, arma::rowvec>();
  // CheckPredictionsType<FFN<>, arma::mat>();

  CheckPredictionsType<LogisticRegression<>, arma::Row<size_t>>();
  CheckPredictionsType<SoftmaxRegression, arma::Row<size_t>>();
  CheckPredictionsType<HoeffdingTree<>, arma::Row<size_t>, arma::mat>();
  CheckPredictionsType<HoeffdingTree<>, arma::Row<size_t>, arma::imat>();
  CheckPredictionsType<DecisionTree<>, arma::Row<size_t>, arma::mat,
      arma::Row<size_t>>();
  CheckPredictionsType<DecisionTree<>, arma::Row<char>, arma::mat,
      arma::Row<char>>();
}

/**
 * Test MetaInfoExtractor correctly identifies whether a given machine learning
 * algorithm supports weighted learning.
 */
BOOST_AUTO_TEST_CASE(SupportsWeightsTest)
{
  static_assert(MetaInfoExtractor<LinearRegression>::SupportsWeights,
      "Value should be true");
  static_assert(MetaInfoExtractor<DecisionTree<>>::SupportsWeights,
      "Value should be true");
  static_assert(MetaInfoExtractor<DecisionTree<>, arma::mat, arma::urowvec,
      arma::Row<float>>::SupportsWeights, "Value should be true");

  static_assert(!MetaInfoExtractor<LARS>::SupportsWeights,
      "Value should be false");
  static_assert(!MetaInfoExtractor<LogisticRegression<>>::SupportsWeights,
      "Value should be false");
}

template<typename Class,
         typename ExpectedWT,
         typename PassedMT = arma::mat,
         typename PassedPT = arma::Row<size_t>,
         typename PassedWT = arma::rowvec>
void CheckWeightsType()
{
  using Extractor = MetaInfoExtractor<Class, PassedMT, PassedPT, PassedWT>;
  using ActualWT = typename Extractor::WeightsType;
  static_assert(std::is_same<ExpectedWT, ActualWT>::value,
      "Should be the same");
}

/**
 * Test MetaInfoExtractor correctly recognizes the type of weights for a given
 * machine learning algorithm.
 */
BOOST_AUTO_TEST_CASE(WeightsTypeTest)
{
  CheckWeightsType<LinearRegression, arma::rowvec>();
  CheckWeightsType<DecisionTree<>, arma::rowvec>();
  CheckWeightsType<DecisionTree<>, arma::Row<float>, arma::mat,
      arma::Row<size_t>, arma::Row<float>>();
}

/**
 * Test MetaInfoExtractor correctly identifies whether a given machine learning
 * algorithm takes a data:DatasetInfo parameter.
 */
BOOST_AUTO_TEST_CASE(TakesDatasetInfoTest)
{
  static_assert(MetaInfoExtractor<DecisionTree<>>::TakesDatasetInfo,
      "Value should be true");
  static_assert(!MetaInfoExtractor<LinearRegression>::TakesDatasetInfo,
      "Value should be false");
  static_assert(!MetaInfoExtractor<SoftmaxRegression>::TakesDatasetInfo,
      "Value should be false");
}

/**
 * Test MetaInfoExtractor correctly identifies whether a given machine learning
 * algorithm takes the numClasses parameter.
 */
BOOST_AUTO_TEST_CASE(TakesNumClassesTest)
{
  static_assert(MetaInfoExtractor<DecisionTree<>>::TakesNumClasses,
      "Value should be true");
  static_assert(MetaInfoExtractor<SoftmaxRegression>::TakesNumClasses,
      "Value should be true");
  static_assert(!MetaInfoExtractor<LinearRegression>::TakesNumClasses,
      "Value should be false");
  static_assert(!MetaInfoExtractor<LARS>::TakesNumClasses,
      "Value should be false");
}

/**
 * Test the simple cross-validation strategy implementation with the Accuracy
 * metric.
 */
BOOST_AUTO_TEST_CASE(SimpleCVAccuracyTest)
{
  // Using the first half of data for training and the rest for validation.
  // The validation labels are 75% correct.
  arma::mat data =
    arma::mat("1 0; 2 0; 1 1; 2 1; 1 0; 2 0; 1 1; 2 1").t();
  arma::Row<size_t> labels("0 0 1 1 0 1 1 1");

  SimpleCV<LogisticRegression<>, Accuracy> cv(0.5, data, labels);

  BOOST_REQUIRE_CLOSE(cv.Evaluate(), 0.75, 1e-5);
}

/**
 * Test the simple cross-validation strategy implementation with the MSE metric.
 */
BOOST_AUTO_TEST_CASE(SimpleCVMSETest)
{
  // Using the first two points for training and remaining three for validation.
  // See the test MSETest for more explanation.
  arma::mat data("0 1 2 3 4");
  arma::rowvec responses("-1 0 1 3 5");

  double expectedMSE = (0 * 0 + 1 * 1 + 2 * 2) / 3.0;

  SimpleCV<LinearRegression, MSE> cv(0.6, data, responses);

  BOOST_REQUIRE_CLOSE(cv.Evaluate(), expectedMSE, 1e-5);

  arma::mat noiseData("-1 -2 -3 -4 -5");
  arma::rowvec noiseResponses("10 20 30 40 50");

  arma::mat allData = arma::join_rows(noiseData, data);
  arma::rowvec allResponces = arma::join_rows(noiseResponses, responses);

  arma::rowvec weights = arma::join_rows(arma::zeros(noiseData.n_cols).t(),
      arma::ones(data.n_cols).t());

  SimpleCV<LinearRegression, MSE> weightedCV(0.3, allData, allResponces,
      weights);

  BOOST_REQUIRE_CLOSE(weightedCV.Evaluate(), expectedMSE, 1e-5);

  arma::rowvec weights2 = arma::join_rows(arma::zeros(noiseData.n_cols - 1).t(),
      arma::ones(data.n_cols + 1).t());

  SimpleCV<LinearRegression, MSE> weightedCV2(0.3, allData, allResponces,
      weights2);

  BOOST_REQUIRE_GT(std::abs(weightedCV2.Evaluate() - expectedMSE), 1e-5);
}

template<typename... DTArgs>
arma::Row<size_t> PredictLabelsWithDT(const arma::mat& data,
                                      const DTArgs&... args)
{
  DecisionTree<InformationGain> dt(args...);
  arma::Row<size_t> predictedLabels;
  dt.Classify(data, predictedLabels);
  return predictedLabels;
}

/**
 * Test the simple cross-validation strategy implementation with decision trees
 * constructed in multiple ways.
 */
BOOST_AUTO_TEST_CASE(SimpleCVWithDTTest)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);

  arma::mat trainingData = data.cols(0, 1999);
  arma::mat testData = data.cols(2000, 3999);
  arma::Row<size_t> trainingLabels = labels.subvec(0, 1999);

  arma::rowvec weights(4000, arma::fill::randu);

  size_t numClasses = 5;
  size_t minimumLeafSize = 8;

  {
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(testData,
        trainingData, trainingLabels, numClasses, minimumLeafSize);
    SimpleCV<DecisionTree<InformationGain>, Accuracy> cv(0.5, data,
        arma::join_rows(trainingLabels, predictedLabels), numClasses);
    BOOST_REQUIRE_CLOSE(cv.Evaluate(minimumLeafSize), 1.0, 1e-5);
  }
  {
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(testData,
        trainingData, datasetInfo, trainingLabels, numClasses, minimumLeafSize);
    SimpleCV<DecisionTree<InformationGain>, Accuracy> cv(0.5, data, datasetInfo,
        arma::join_rows(trainingLabels, predictedLabels), numClasses);
    BOOST_REQUIRE_CLOSE(cv.Evaluate(minimumLeafSize), 1.0, 1e-5);
  }
  {
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(testData,
        trainingData, trainingLabels, numClasses, weights, minimumLeafSize);
    SimpleCV<DecisionTree<InformationGain>, Accuracy> cv(0.5, data,
        arma::join_rows(trainingLabels, predictedLabels), numClasses, weights);
    BOOST_REQUIRE_CLOSE(cv.Evaluate(minimumLeafSize), 1.0, 1e-5);
  }
  {
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(testData,
        trainingData, datasetInfo, trainingLabels, numClasses, weights,
        minimumLeafSize);
    SimpleCV<DecisionTree<InformationGain>, Accuracy> cv(0.5, data, datasetInfo,
        arma::join_rows(trainingLabels, predictedLabels), numClasses, weights);
    BOOST_REQUIRE_CLOSE(cv.Evaluate(minimumLeafSize), 1.0, 1e-5);
  }
}

/**
 * Test k-fold cross-validation with the MSE metric.
 */
BOOST_AUTO_TEST_CASE(KFoldCVMSETest)
{
  // Defining dataset with two sets of responses for the same two data points.
  arma::mat data("0 1  0 1");
  arma::rowvec responses("0 1  1 3");

  // 2-fold cross-validation, no shuffling.
  KFoldCV<LinearRegression, MSE> cv(2, data, responses, false);

  // In each of two validation tests the MSE value should be the same.
  double expectedMSE =
      double((1 - 0) * (1 - 0) + (3 - 1) * (3 - 1)) / 2 * 2 / 2;

  BOOST_REQUIRE_CLOSE(cv.Evaluate(), expectedMSE, 1e-5);

  // Assert we can access a trained model without the exception of
  // uninitialization.
  cv.Model();
}

/**
 * Test k-fold cross-validation with the Accuracy metric.
 */
BOOST_AUTO_TEST_CASE(KFoldCVAccuracyTest)
{
  // Making a 10-points dataset. The last point should be classified wrong when
  // it is tested separately.
  arma::mat data("0 1 2 3 100 101 102 103 104 5");
  arma::Row<size_t> labels("0 0 0 0 1 1 1 1 1 1");
  size_t numClasses = 2;

  // 10-fold cross-validation, no shuffling.
  KFoldCV<NaiveBayesClassifier<>, Accuracy> cv(10, data, labels, numClasses,
      false);

  // We should succeed in classifying separately the first nine samples, and
  // fail with the remaining one.
  double expectedAccuracy = (9 * 1.0 + 0.0) / 10;

  BOOST_REQUIRE_CLOSE(cv.Evaluate(), expectedAccuracy, 1e-5);

  // Assert we can access a trained model without the exception of
  // uninitialization.
  cv.Model();
}

/**
 * Test k-fold cross-validation with weighted linear regression.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithWeightedLRTest)
{
  // Each fold will be filled with this dataset.
  arma::mat data("1 2 3 4");
  arma::rowvec responses("1 2 30 40");
  arma::rowvec weights("1 1 0 0");

  KFoldCV<LinearRegression, MSE> cv(2, arma::join_rows(data, data),
      arma::join_rows(responses, responses), arma::join_rows(weights, weights),
      false);
  cv.Evaluate();

  arma::mat testData("3 4");
  arma::rowvec testResponses("3 4");

  double mse = MSE::Evaluate(cv.Model(), testData, testResponses);

  BOOST_REQUIRE_CLOSE(1.0 - mse, 1.0, 1e-5);
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTest)
{
  arma::mat originalData;
  arma::Row<size_t> originalLabels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(originalData, originalLabels, datasetInfo);

  // Each fold will be filled with this dataset.
  arma::mat data = originalData.cols(0, 1199);
  arma::Row<size_t> labels = originalLabels.cols(0, 1199);
  arma::rowvec weights(data.n_cols, arma::fill::randu);

  arma::mat doubledData = arma::join_rows(data, data);
  arma::Row<size_t> doubledLabels = arma::join_rows(labels, labels);
  arma::rowvec doubledWeights = arma::join_rows(weights, weights);

  size_t numClasses = 5;
  size_t minimumLeafSize = 8;

  {
    KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(2, doubledData,
        doubledLabels, numClasses, false);
    cv.Evaluate(minimumLeafSize);
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(data, data, labels,
        numClasses, minimumLeafSize);
    double accuracy = Accuracy::Evaluate(cv.Model(), data, predictedLabels);
    BOOST_REQUIRE_CLOSE(accuracy, 1.0, 1e-5);
  }
  {
    KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(2, doubledData,
        datasetInfo, doubledLabels, numClasses, false);
    cv.Evaluate(minimumLeafSize);
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(data, data,
        datasetInfo, labels, numClasses, minimumLeafSize);
    double accuracy = Accuracy::Evaluate(cv.Model(), data, predictedLabels);
    BOOST_REQUIRE_CLOSE(accuracy, 1.0, 1e-5);
  }
  {
    KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(2, doubledData,
        doubledLabels, numClasses, doubledWeights, false);
    cv.Evaluate(minimumLeafSize);
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(data, data, labels,
        numClasses, weights, minimumLeafSize);
    double accuracy = Accuracy::Evaluate(cv.Model(), data, predictedLabels);
    BOOST_REQUIRE_CLOSE(accuracy, 1.0, 1e-5);
  }
  {
    KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(2, doubledData,
        datasetInfo, doubledLabels, numClasses, doubledWeights, false);
    cv.Evaluate(minimumLeafSize);
    arma::Row<size_t> predictedLabels = PredictLabelsWithDT(data, data,
        datasetInfo, labels, numClasses, weights, minimumLeafSize);
    double accuracy = Accuracy::Evaluate(cv.Model(), data, predictedLabels);
    BOOST_REQUIRE_CLOSE(accuracy, 1.0, 1e-5);
  }
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways, but with larger k and no shuffling.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTestLargeKNoShuffle)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);

  size_t numClasses = 5;
  size_t minimumLeafSize = 5;

  KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(5, data,
      datasetInfo, labels, numClasses, false);
  cv.Evaluate(minimumLeafSize);
  double accuracy = Accuracy::Evaluate(cv.Model(), data, labels);

  // This is a very loose tolerance, but we expect about the same as we would
  // from an individual decision tree training.
  BOOST_REQUIRE_GT(accuracy, 0.7);
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways, but with larger k such that the number of points in each
 * cross-validation bin is not even (the last is smaller), and also with no
 * shuffling.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTestUnevenBinsNoShuffle)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);

  size_t numClasses = 5;
  size_t minimumLeafSize = 5;

  KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(7, data, datasetInfo,
      labels, numClasses, false);
  cv.Evaluate(minimumLeafSize);
  double accuracy = Accuracy::Evaluate(cv.Model(), data, labels);

  // This is a very loose tolerance, but we expect about the same as we would
  // from an individual decision tree training.
  BOOST_REQUIRE_GT(accuracy, 0.7);
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways, but with larger k.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTestLargeK)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);

  size_t numClasses = 5;
  size_t minimumLeafSize = 5;

  KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(5, data,
      datasetInfo, labels, numClasses);
  cv.Evaluate(minimumLeafSize);
  double accuracy = Accuracy::Evaluate(cv.Model(), data, labels);

  // This is a very loose tolerance, but we expect about the same as we would
  // from an individual decision tree training.
  BOOST_REQUIRE_GT(accuracy, 0.7);
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways, but with larger k such that the number of points in each
 * cross-validation bin is not even (the last is smaller).
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTestUnevenBins)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);

  size_t numClasses = 5;
  size_t minimumLeafSize = 5;

  KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(7, data, datasetInfo,
      labels, numClasses);
  cv.Evaluate(minimumLeafSize);
  double accuracy = Accuracy::Evaluate(cv.Model(), data, labels);

  // This is a very loose tolerance, but we expect about the same as we would
  // from an individual decision tree training.
  BOOST_REQUIRE_GT(accuracy, 0.7);
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways, but with larger k and weights.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTestLargeKWeighted)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);
  arma::rowvec weights(data.n_cols, arma::fill::randu);

  size_t numClasses = 5;
  size_t minimumLeafSize = 5;

  KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(5, data,
      datasetInfo, labels, numClasses, weights);
  cv.Evaluate(minimumLeafSize);
  double accuracy = Accuracy::Evaluate(cv.Model(), data, labels);

  // This is a very loose tolerance, but we expect about the same as we would
  // from an individual decision tree training.
  BOOST_REQUIRE_GT(accuracy, 0.7);
}

/**
 * Test k-fold cross-validation with decision trees constructed in multiple
 * ways, but with larger k such that the number of points in each
 * cross-validation bin is not even (the last is smaller) and weights.
 */
BOOST_AUTO_TEST_CASE(KFoldCVWithDTTestUnevenBinsWeighted)
{
  arma::mat data;
  arma::Row<size_t> labels;
  data::DatasetInfo datasetInfo;
  MockCategoricalData(data, labels, datasetInfo);
  arma::rowvec weights(data.n_cols, arma::fill::randu);

  size_t numClasses = 5;
  size_t minimumLeafSize = 5;

  KFoldCV<DecisionTree<InformationGain>, Accuracy> cv(7, data, datasetInfo,
      labels, numClasses, weights);
  cv.Evaluate(minimumLeafSize);
  double accuracy = Accuracy::Evaluate(cv.Model(), data, labels);

  // This is a very loose tolerance, but we expect about the same as we would
  // from an individual decision tree training.
  BOOST_REQUIRE_GT(accuracy, 0.7);
}

BOOST_AUTO_TEST_SUITE_END();


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing tan capabilities

    tangent of the input in radians.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = tan(x);
    @endcode

    is similar to:

    @code
    T r = sin(x)/cos(x);
    @endcode

    As most other trigonometric function tan can be called with a second optional parameter
    which is a tag on speed and accuracy (see @ref cos for further details)

    @see tand, tanpi

  **/
  const boost::dispatch::functor<tag::tan_> tan = {};
} }
#endif

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

#endif


#pragma once

#include <polyfem/ElasticityUtils.hpp>
#include <polyfem/AutodiffTypes.hpp>
#include <Eigen/Dense>

namespace polyfem {
namespace autogen {
void linear_elasticity_2d_function(const AutodiffHessianPt &pt, const double lambda, const double mu, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);
void linear_elasticity_3d_function(const AutodiffHessianPt &pt, const double lambda, const double mu, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);

void hooke_2d_function(const AutodiffHessianPt &pt, const ElasticityTensor &C, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);
void hooke_3d_function(const AutodiffHessianPt &pt, const ElasticityTensor &C, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);

void saint_venant_2d_function(const AutodiffHessianPt &pt, const ElasticityTensor &C, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);
void saint_venant_3d_function(const AutodiffHessianPt &pt, const ElasticityTensor &C, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);

void neo_hookean_2d_function(const AutodiffHessianPt &pt, const double lambda, const double mu, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);
void neo_hookean_3d_function(const AutodiffHessianPt &pt, const double lambda, const double mu, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> &res);

}}