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

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <vector>
#include <array>
#include <boost/random/uniform_real.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/interpolators/quintic_hermite.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/circular_buffer.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::quintic_hermite;
using boost::math::interpolators::cardinal_quintic_hermite;
using boost::math::interpolators::cardinal_quintic_hermite_aos;

template<typename Real>
void test_constant()
{
    std::vector<Real> x{0,1,2,3, 9, 22, 81};
    std::vector<Real> y(x.size());
    std::vector<Real> dydx(x.size(), 0);
    std::vector<Real> d2ydx2(x.size(), 0);
    for (auto & t : y)
    {
        t = 7;
    }

    auto qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));
    for (Real t = 0; t <= 81; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(7), qh(t), 24);
        CHECK_ULP_CLOSE(Real(0), qh.prime(t), 24);
        CHECK_ULP_CLOSE(Real(0), qh.double_prime(t), 24);
    }
}

template<typename Real>
void test_linear()
{
    std::vector<Real> x{0,1,2,3, 4,5,6,7,8,9};
    std::vector<Real> y = x;
    std::vector<Real> dydx(x.size(), 1);
    std::vector<Real> d2ydx2(x.size(), 0);

    auto qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));

    for (Real t = 0; t <= 9; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(t), qh(t), 2);
        CHECK_ULP_CLOSE(Real(1), qh.prime(t), 2);
        CHECK_ULP_CLOSE(Real(0), qh.double_prime(t), 2);
    }

    boost::random::mt19937 rng;
    boost::random::uniform_real_distribution<Real> dis(0.5,1);
    x.resize(512);
    x[0] = dis(rng);
    Real xmin = x[0];
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(rng);
    }
    Real xmax = x.back();

    y = x;
    dydx.resize(x.size(), 1);
    d2ydx2.resize(x.size(), 0);

    qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));

    for (Real t = xmin; t <= xmax; t += 0.125)
    {
        CHECK_ULP_CLOSE(t, qh(t), 2);
        CHECK_ULP_CLOSE(Real(1), qh.prime(t), 100);
        CHECK_MOLLIFIED_CLOSE(Real(0), qh.double_prime(t), 200*std::numeric_limits<Real>::epsilon());
    }
}

template<typename Real>
void test_quadratic()
{

    std::vector<Real> x{0,1,2,3, 4,5,6,7,8,9};
    std::vector<Real> y(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]/2;
    }

    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < y.size(); ++i) {
        dydx[i] = x[i];
    }

    std::vector<Real> d2ydx2(x.size(), 1);

    auto qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));

    for (Real t = 0; t <= 9; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(Real(t*t)/2, qh(t), 2);
        CHECK_ULP_CLOSE(t, qh.prime(t), 12);
        CHECK_ULP_CLOSE(Real(1), qh.double_prime(t), 32);
    }

    boost::random::mt19937 rng;
    boost::random::uniform_real_distribution<Real> dis(0.5,1);
    x.resize(8);
    x[0] = dis(rng);
    Real xmin = x[0];
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(rng);
    }
    Real xmax = x.back();

    y.resize(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]/2;
    }

    dydx.resize(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = x[i];
    }

    d2ydx2.resize(x.size(), 1);

    qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));

    for (Real t = xmin; t <= xmax; t += 0.125)
    {
        CHECK_ULP_CLOSE(Real(t*t)/2, qh(t), 4);
        CHECK_ULP_CLOSE(t, qh.prime(t), 53);
        CHECK_ULP_CLOSE(Real(1), qh.double_prime(t), 700);
    }
}

template<typename Real>
void test_cubic()
{

    std::vector<Real> x{0,1,2,3, 4,5,6,7,8,9};
    std::vector<Real> y(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]*x[i];
    }

    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < y.size(); ++i) {
        dydx[i] = 3*x[i]*x[i];
    }

    std::vector<Real> d2ydx2(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        d2ydx2[i] = 6*x[i];
    }

    auto qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));

    for (Real t = 0; t <= 9; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t, qh(t), 10);
        CHECK_ULP_CLOSE(3*t*t, qh.prime(t), 15);
        CHECK_ULP_CLOSE(6*t, qh.double_prime(t), 20);
    }
}

template<typename Real>
void test_quartic()
{

    std::vector<Real> x{0,1,2,3, 4,5,6,7,8,9, 10, 11};
    std::vector<Real> y(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]*x[i]*x[i];
    }

    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = 4*x[i]*x[i]*x[i];
    }

    std::vector<Real> d2ydx2(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        d2ydx2[i] = 12*x[i]*x[i];
    }

    auto qh = quintic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2));

    for (Real t = 1; t <= 11; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t*t, qh(t), 100);
        CHECK_ULP_CLOSE(4*t*t*t, qh.prime(t), 100);
        CHECK_ULP_CLOSE(12*t*t, qh.double_prime(t), 100);
    }
}

template<typename Real>
void test_interpolation_condition()
{
    for (size_t n = 4; n < 50; ++n) {
        std::vector<Real> x(n);
        std::vector<Real> y(n);
        std::vector<Real> dydx(n);
        std::vector<Real> d2ydx2(n);
        boost::random::mt19937 rd;
        boost::random::uniform_real_distribution<Real> dis(0,1);
        Real x0 = dis(rd);
        x[0] = x0;
        y[0] = dis(rd);
        for (size_t i = 1; i < n; ++i) {
            x[i] = x[i-1] + dis(rd);
            y[i] = dis(rd);
            dydx[i] = dis(rd);
            d2ydx2[i] = dis(rd);
        }

        auto x_copy = x;
        auto y_copy = y;
        auto dydx_copy = dydx;
        auto d2ydx2_copy = d2ydx2;
        auto s = quintic_hermite(std::move(x_copy), std::move(y_copy), std::move(dydx_copy), std::move(d2ydx2_copy));
        //std::cout << "s = " << s << "\n";
        for (size_t i = 0; i < x.size(); ++i) {
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
            CHECK_ULP_CLOSE(dydx[i], s.prime(x[i]), 2);
            CHECK_ULP_CLOSE(d2ydx2[i], s.double_prime(x[i]), 2);
        }
    }
}

template<typename Real>
void test_cardinal_constant()
{

    std::vector<Real> y(25);
    std::vector<Real> dydx(y.size(), 0);
    std::vector<Real> d2ydx2(y.size(), 0);
    for (auto & t : y) {
        t = 7;
    }
    Real x0 = 4;
    Real dx = Real(1)/Real(8);

    auto qh = cardinal_quintic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), x0, dx);

    for (Real t = x0; t <= x0 + 24*dx; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(7), qh(t), 24);
        CHECK_ULP_CLOSE(Real(0), qh.prime(t), 24);
        CHECK_ULP_CLOSE(Real(0), qh.double_prime(t), 24);
    }

    std::vector<std::array<Real, 3>> data(25);
    for (size_t i = 0; i < data.size(); ++i)
    {
        data[i][0] = 7;
        data[i][1] = 0;
        data[i][2] = 0;
    }

    auto qh_aos = cardinal_quintic_hermite_aos(std::move(data), x0, dx);
    for (Real t = x0; t <= x0 + 24*dx; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(7), qh_aos(t), 24);
        CHECK_ULP_CLOSE(Real(0), qh_aos.prime(t), 24);
        CHECK_ULP_CLOSE(Real(0), qh_aos.double_prime(t), 24);
    }

    // Now check the boundaries:
    auto [tlo, thi] = qh.domain();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(7), qh(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), qh(thi), 2);
        CHECK_ULP_CLOSE(Real(7), qh_aos(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), qh_aos(thi), 2);
        CHECK_ULP_CLOSE(Real(0), qh.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), qh.prime(thi), 2);
        CHECK_ULP_CLOSE(Real(0), qh_aos.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), qh_aos.prime(thi), 2);

        tlo = boost::math::nextafter(tlo, std::numeric_limits<Real>::max());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }
}

template<typename Real>
void test_cardinal_linear()
{
    std::vector<Real> y{0,1,2,3,4,5,6,7,8,9};
    Real x0 = 0;
    Real dx = 1;
    std::vector<Real> dydx(y.size(), 1);
    std::vector<Real> d2ydx2(y.size(), 0);

    auto qh = cardinal_quintic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), x0, dx);

    for (Real t = 0; t <= 9; t += 0.25) {
        CHECK_ULP_CLOSE(Real(t), qh(t), 2);
        CHECK_ULP_CLOSE(Real(1), qh.prime(t), 2);
        CHECK_ULP_CLOSE(Real(0), qh.double_prime(t), 2);
    }

    std::vector<std::array<Real, 3>> data(10);
    for (size_t i = 0; i < data.size(); ++i) {
        data[i][0] = i;
        data[i][1] = 1;
        data[i][2] = 0;
    }

    auto qh_aos = cardinal_quintic_hermite_aos(std::move(data), x0, dx);

    for (Real t = 0; t <= 9; t += 0.25) {
        CHECK_ULP_CLOSE(Real(t), qh_aos(t), 2);
        CHECK_ULP_CLOSE(Real(1), qh_aos.prime(t), 2);
        CHECK_ULP_CLOSE(Real(0), qh_aos.double_prime(t), 2);
    }

    // Now check the boundaries:
    auto [tlo, thi] = qh.domain();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(tlo), qh(tlo), 2);
        CHECK_ULP_CLOSE(Real(thi), qh(thi), 2);
        CHECK_ULP_CLOSE(Real(tlo), qh_aos(tlo), 2);
        CHECK_ULP_CLOSE(Real(thi), qh_aos(thi), 2);
        CHECK_ULP_CLOSE(Real(1), qh.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(1), qh.prime(thi), 128);
        CHECK_ULP_CLOSE(Real(1), qh_aos.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(1), qh_aos.prime(thi), 128);

        tlo = boost::math::nextafter(tlo, std::numeric_limits<Real>::max());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }
}

template<typename Real>
void test_cardinal_quadratic()
{
    Real x0 = 0;
    Real dx = 1;
    std::vector<Real> y(10);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i*i/Real(2);
    }

    std::vector<Real> dydx(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        dydx[i] = i;
    }

    std::vector<Real> d2ydx2(y.size(), 1);

    auto qh = cardinal_quintic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), x0, dx);

    for (Real t = 0; t <= 9; t += 0.0078125) {
        Real computed = qh(t);
        CHECK_ULP_CLOSE(Real(t*t)/2, computed, 2);
        CHECK_ULP_CLOSE(t, qh.prime(t), 15);
        CHECK_ULP_CLOSE(Real(1), qh.double_prime(t), 32);
    }

    std::vector<std::array<Real, 3>> data(10);
    for (size_t i = 0; i < data.size(); ++i) {
        data[i][0] = i*i/Real(2);
        data[i][1] = i;
        data[i][2] = 1;
    }
    auto qh_aos = cardinal_quintic_hermite_aos(std::move(data), x0, dx);

    for (Real t = 0; t <= 9; t += 0.0078125)
    {
        Real computed = qh_aos(t);
        CHECK_ULP_CLOSE(Real(t*t)/2, computed, 2);
        CHECK_ULP_CLOSE(t, qh_aos.prime(t), 12);
        CHECK_ULP_CLOSE(Real(1), qh_aos.double_prime(t), 64);
    }

        // Now check the boundaries:
    auto [tlo, thi] = qh.domain();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(tlo*tlo/2, qh(tlo), 16);
        CHECK_ULP_CLOSE(thi*thi/2, qh(thi), 16);
        CHECK_ULP_CLOSE(tlo*tlo/2, qh_aos(tlo), 16);
        CHECK_ULP_CLOSE(thi*thi/2, qh_aos(thi), 16);
        CHECK_ULP_CLOSE(tlo, qh.prime(tlo), 16);
        CHECK_ULP_CLOSE(thi, qh.prime(thi), 64);
        CHECK_ULP_CLOSE(tlo, qh_aos.prime(tlo), 16);
        CHECK_ULP_CLOSE(thi, qh_aos.prime(thi), 64);

        tlo = boost::math::nextafter(tlo, std::numeric_limits<Real>::max());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }
}

template<typename Real>
void test_cardinal_cubic()
{
    Real x0 = 0;
    Real dx = 1;
    std::vector<Real> y(10);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i*i*i;
    }

    std::vector<Real> dydx(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        dydx[i] = 3*i*i;
    }

    std::vector<Real> d2ydx2(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        d2ydx2[i] = 6*i;
    }

    auto qh = cardinal_quintic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), x0, dx);

    for (Real t = 0; t <= 9; t += 0.0078125)
    {
        Real computed = qh(t);
        CHECK_ULP_CLOSE(t*t*t, computed, 10);
        CHECK_ULP_CLOSE(3*t*t, qh.prime(t), 15);
        CHECK_ULP_CLOSE(6*t, qh.double_prime(t), 39);
    }

    std::vector<std::array<Real, 3>> data(10);
    for (size_t i = 0; i < data.size(); ++i) {
        data[i][0] = i*i*i;
        data[i][1] = 3*i*i;
        data[i][2] = 6*i;
    }

    auto qh_aos = cardinal_quintic_hermite_aos(std::move(data), x0, dx);
    for (Real t = 0; t <= 9; t += 0.0078125)
    {
        Real computed = qh_aos(t);
        CHECK_ULP_CLOSE(t*t*t, computed, 10);
        CHECK_ULP_CLOSE(3*t*t, qh_aos.prime(t), 15);
        CHECK_ULP_CLOSE(6*t, qh_aos.double_prime(t), 30);
    }
}

template<typename Real>
void test_cardinal_quartic()
{
    Real x0 = 0;
    Real dx = 1;
    std::vector<Real> y(7);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i*i*i*i;
    }

    std::vector<Real> dydx(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        dydx[i] = 4*i*i*i;
    }

    std::vector<Real> d2ydx2(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        d2ydx2[i] = 12*i*i;
    }

    auto qh = cardinal_quintic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), x0, dx);

    for (Real t = 0; t <= 6; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(Real(t*t*t*t), qh(t), 250);
        CHECK_ULP_CLOSE(4*t*t*t, qh.prime(t), 250);
        CHECK_ULP_CLOSE(12*t*t, qh.double_prime(t), 250);
    }

    std::vector<std::array<Real, 3>> data(7);
    for (size_t i = 0; i < data.size(); ++i) {
        data[i][0] = i*i*i*i;
        data[i][1] = 4*i*i*i;
        data[i][2] = 12*i*i;
    }

    auto qh_aos = cardinal_quintic_hermite_aos(std::move(data), x0, dx);
    for (Real t = 0; t <= 6; t += 0.0078125)
    {
        Real computed = qh_aos(t);
        CHECK_ULP_CLOSE(t*t*t*t, computed, 10);
        CHECK_ULP_CLOSE(4*t*t*t, qh_aos.prime(t), 64);
        CHECK_ULP_CLOSE(12*t*t, qh_aos.double_prime(t), 128);
    }
}

int main()
{
    test_constant<float>();
    test_linear<float>();
    test_quadratic<float>();
    test_cubic<float>();
    test_quartic<float>();
    test_interpolation_condition<float>();

    test_cardinal_constant<float>();
    test_cardinal_linear<float>();
    test_cardinal_quadratic<float>();
    test_cardinal_cubic<float>();
    test_cardinal_quartic<float>();

    test_constant<double>();
    test_linear<double>();
    test_quadratic<double>();
    test_cubic<double>();
    test_quartic<double>();
    test_interpolation_condition<double>();

    test_cardinal_constant<double>();
    test_cardinal_linear<double>();
    test_cardinal_quadratic<double>();
    test_cardinal_cubic<double>();
    test_cardinal_quartic<double>();

    test_constant<long double>();
    test_linear<long double>();
    test_quadratic<long double>();
    test_cubic<long double>();
    test_quartic<long double>();
    test_interpolation_condition<long double>();

    test_cardinal_constant<long double>();
    test_cardinal_linear<long double>();
    test_cardinal_quadratic<long double>();
    test_cardinal_cubic<long double>();
    test_cardinal_quartic<long double>();

#ifdef BOOST_HAS_FLOAT128
    test_constant<float128>();
    //test_linear<float128>();
    test_quadratic<float128>();
    test_cubic<float128>();
    test_quartic<float128>();
    test_interpolation_condition<float128>();
    test_cardinal_constant<float128>();
    test_cardinal_linear<float128>();
    test_cardinal_quadratic<float128>();
    test_cardinal_cubic<float128>();
    test_cardinal_quartic<float128>();
#endif

    return boost::math::test::report_errors();
}


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

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

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto numbers = integer_list<int, 5, -1, 0, -7, -2, 0, -5, 4>;
    BOOST_HANA_CONSTEXPR_LAMBDA auto negatives = integer_list<int, -1, -7, -2, -5>;

    BOOST_HANA_CONSTEXPR_LAMBDA auto keep_negatives = [](auto n, auto acc) {
        return if_(n < 0_c, cons(n, acc), acc);
    };

    BOOST_HANA_CONSTANT_ASSERT(
        foldr(numbers, integer_list<int>, keep_negatives) == negatives
    );
    //! [main]
}


/**
 * Functions for optimizing functions.
 * Includes line search to find a step length to reduce a function.
 */

#pragma once

#include <Eigen/Core>

namespace ccd {
namespace opt {

    /**
     * @brief Search along a search direction to find a scalar \f$\alpha
     * \in [0, 1]\f$ such that \f$f(x + \alpha \Delta x) \leq f(x)\f$.
     *
     * @param[in] x                Starting point for the line search.
     * @param[in] dir              Direction to search along.
     * @param[in] f                Function of x to minimize.
     * @param[out] step_length     Scalar coefficent of the direction to step.
     * @param[in] min_step_length  Minimum value of step_length before the line
     *                             search fails.
     *
     * @return True if the line search was successful, false otherwise.
     */
    bool line_search(
        const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        double& step_length,
        const double min_step_length = 1e-10);

    /**
     * @brief Search along a search direction to find a scalar \f$\alpha
     * \in [0, 1]\f$ such that \f$f(x + \alpha \Delta x) < f(x)\f$.
     *
     * @param[in] x                Starting point for the line search.
     * @param[in] dir              Direction to search along.
     * @param[in] f                Function of x to minimize.
     * @param[in] grad_fx          The precomputed value of \f$\nabla f(x)\f$.
     * @param[out] step_length     Scalar coefficent of the direction to step.
     * @param[in] min_step_length  Minimum value of step_length before the line
     *                             search fails.
     *
     * @return True if the line search was successful, false otherwise.
     */
    bool line_search(
        const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        const Eigen::VectorXd& grad_fx,
        double& step_length,
        const double min_step_length = 1e-10,
        const double armijo_rule_coeff = 0);

    /**
     * @brief Search along a search direction to find a scalar \f$\alpha
     * \in [0, 1]\f$ such that \f$f(x + \alpha \Delta x) < f(x)\f$.
     *
     * @param[in] x                Starting point for the line search.
     * @param[in] dir              Direction to search along \f$(\Delta x)\f$.
     * @param[in] f                Function of x to minimize.
     * @param[in] grad_fx          The precomputed value of \f$\nabla f(x)\f$.
     * @param[in] constraint       Constraint on x such that constraint(x) must
     *                             be true.
     * @param[out] step_length     Scalar coefficent of the direction to step
     *                             \f$(\alpha)\f$.
     * @param[in] min_step_length  Minimum value of step_length before the line
     *                             search fails.
     *
     * @return True if the line search was successful, false otherwise.
     */
    bool constrained_line_search(
        const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const std::function<double(const Eigen::VectorXd&)>& f,
        const Eigen::VectorXd& grad_fx,
        const std::function<bool(const Eigen::VectorXd&)>& constraint,
        double& step_length,
        const double min_step_length = 1e-10,
        const double armijo_rule_coeff = 0);

} // namespace opt
} // namespace ccd


/**
 * @file calc-jump.cpp
 *
 * @brief calc jump function.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 *
 * Compile:
 * g++ calc-jump.cpp -o calc-jump -lntl
 *
 * Compute polynomial for 2^128 steps:
 * ./calc-jump 340282366920938463463374607431768211456 poly.19937.txt
 *
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/ZZ.h>
#include "dsfmt-calc-jump.hpp"

using namespace NTL;
using namespace std;
using namespace dsfmt;

static void read_file(GF2X& lcmpoly, long line_no, const string& file);

int main(int argc, char * argv[]) {
    if (argc <= 2) {
	cout << argv[0] << " jump-step poly-file" << endl;
	cout << "    jump-step: a number between zero and 2^{DSFMT_MEXP}-1.\n"
	     << "               large decimal number is allowed." << endl;
	cout << "    poly-file: one of poly.{MEXP}.txt "
	     << "file" << endl;
	return -1;
    }
    string step_string = argv[1];
    string filename = argv[2];
    long no = 0;
    GF2X lcmpoly;
    read_file(lcmpoly, no, filename);
    ZZ step;
    stringstream ss(step_string);
    ss >> step;
    string jump_str;
    calc_jump(jump_str, step, lcmpoly);
    cout << "jump polynomial:" << endl;
    cout << jump_str << endl;
    return 0;
}

static void read_file(GF2X& lcmpoly, long line_no, const string& file)
{
    ifstream ifs(file.c_str());
    string line;
    for (int i = 0; i < line_no; i++) {
	ifs >> line;
	ifs >> line;
    }
    if (ifs) {
	ifs >> line;
	line = "";
	ifs >> line;
    }
    stringtopoly(lcmpoly, line);
}


/**
 * @date Fri Jan 27 14:10:23 2012 +0100
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <boost/shared_array.hpp>

#include <bob.math/inv.h>

#include <bob.math/linear.h>

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>

// Declaration of the external LAPACK function
// LU decomposition of a general matrix (dgetrf)
extern "C" void dgetrf_( const int *M, const int *N, double *A, const int *lda,
  int *ipiv, int *info);
// Inverse of a general matrix (dgetri)
extern "C" void dgetri_( const int *N, double *A, const int *lda,
  const int *ipiv, double *work, const int *lwork, int *info);

void bob::math::inv(const blitz::Array<double,2>& A, blitz::Array<double,2>& B)
{
  // Size variable
  const int N = A.extent(0);
  const blitz::TinyVector<int,2> shapeA(N,N);
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(B);

  bob::core::array::assertSameShape(A,shapeA);
  bob::core::array::assertSameShape(B,shapeA);

  bob::math::inv_(A, B);
}

void bob::math::inv_(const blitz::Array<double,2>& A, blitz::Array<double,2>& B)
{
  // Size variable
  const int N = A.extent(0);

  //////////////////////////////////////
  // Prepares to call LAPACK functions
  // Initializes LAPACK variables
  int info = 0;
  const int lda = N;

  // Initializes LAPACK arrays
  boost::shared_array<int> ipiv(new int[N]);

  // Tries to use B directly if possible
  //   Input and output arrays are both column-major order.
  //   Hence, we can ignore the problem of column- and row-major order
  //   conversions.
  bool B_direct_use = bob::core::array::isCZeroBaseContiguous(B);
  blitz::Array<double,2> A_blitz_lapack;
  if (B_direct_use)
  {
    A_blitz_lapack.reference(B);
    A_blitz_lapack = A;
  }
  else
    A_blitz_lapack.reference(bob::core::array::ccopy(A));
  double *A_lapack = A_blitz_lapack.data();

  // Calls the LAPACK functions
  // 1/ Computes the LU decomposition
  dgetrf_( &N, &N, A_lapack, &lda, ipiv.get(), &info);
  // Checks the info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dgetrf function returned a non-zero value.");

  // TODO: We might consider adding a real invertibility test as described in
  // this thread (Btw, this is what matlab does):
  // http://icl.cs.utk.edu/lapack-forum/archives/lapack/msg00778.html

  // 2/ Computes the inverse matrix
  // 2/A/ Queries the optimal size of the working array
  const int lwork_query = -1;
  double work_query;
  dgetri_( &N, A_lapack, &lda, ipiv.get(), &work_query, &lwork_query, &info);
  // 2/B/ Computes the inverse
  const int lwork = static_cast<int>(work_query);
  boost::shared_array<double> work(new double[lwork]);
  dgetri_( &N, A_lapack, &lda, ipiv.get(), work.get(), &lwork, &info);
  // Checks info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK dgetri function returned a non-zero value. The matrix might not be invertible.");

  // Copy back content to B if required
  if (!B_direct_use)
    B = A_blitz_lapack;
}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Dense>
#include <queue>
// #include<Eigen/SparseCore>
using namespace Rcpp;
using namespace Eigen;
using namespace std;

// [[Rcpp::depends(RcppEigen)]]

//
using Eigen::Map;               	// 'maps' rather than copies
using Eigen::Matrix;                  //  matrix generic
using Eigen::MatrixXd;                  // variable size matrix, double precision
using Eigen::VectorXd;                  // variable size vector, double precision
using Eigen::Transpositions;
using Eigen::HouseholderQR;    // Fast scalable QR solver
using Eigen::ColPivHouseholderQR;    // Fast scalable QR solver
using Eigen::FullPivHouseholderQR; // slow full (colsand rows pivoting)
using Eigen::JacobiSVD;
using Eigen::GeneralizedSelfAdjointEigenSolver;    // one of the eigenvalue solvers
using Eigen::SelfAdjointEigenSolver;    // one of the eigenvalue solvers
using Eigen::LLT;
using Eigen::LDLT;
using Rcpp::List;
using Rcpp::wrap;

// ##########  OK vrsione Sept 04 works

// copied to fspca_sept.cpp

// =========================================================================

// creates a sub-mat of S with indices in e
Eigen::MatrixXd makeSubS(Eigen::MatrixXd S, Eigen::VectorXi e){
  int n = S.cols();
  int r = S.rows();
  int d = e.size();
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  Eigen::MatrixXd M(r, d );
  for (int i = 0; i < d; ++i){
    M.col(i) = S.col(e(i));
  }
  for (int i = 0; i < d; ++i){
    M.row(i) = M.row(e(i));
  }

  return M.topLeftCorner(d, d);
}

// retruns the rows in e and keeps first c columns
Eigen::MatrixXd selectRowsC(Eigen::MatrixXd A, Eigen::VectorXi e, int c){
  // ATTENZIONE INDICES BASE 0
  // ATTENZIONE e must be sorted e(0) < e(1)

  int n = A.cols();
  int r = A.rows();
  int d = e.size();
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  Eigen::MatrixXd M(A.topLeftCorner(r,c));
  for (int i = 0; i < d; ++i){
    M.row(i) = M.row(e(i));
  }

  return M.topLeftCorner(d, c);
}

// Deflates S and D (pass already deflated and vector current loads)
// returns vexp by ref
void deflSandDC(Eigen::VectorXd a, Eigen::MatrixXd& K,
                Eigen::MatrixXd& D, Eigen::VectorXi ind, double& vexp){
  // # pass only a nonzero loads
  // S = deflated matrix
  // # D = SS
  // #  K <-- (S - Saa'S/(a'Sa) // deflated S matrix
  // # KK deflated product corr matrix D = KK
  // #   KK = D - Daa'S/(a'Sa) - Saa'D/(a'Sa) + Saa'Daa'S/(a'Sa)^2
  // ## ===
  const int n = ind.size();
  const int p = K.cols();

  // t = Sa
  Eigen::VectorXd t = Eigen::VectorXd::Zero(p);
  for (int i = 0; i < p; i++)
    for(int k = 0; k < n; k++)
      t(i) += K(i, ind(k)) * a(k ); // only elements in ind
  // tt = a'Sa = t'a

  double tt = 0.0;
  for(int k = 0; k < n; k++)
    tt += a(k) * t(ind(k));
  if (tt > 0)
    tt = 1/tt;
  else
    Rf_error("defSandD: tt is not > 0");

  // O = Sa/(tt)
  const Eigen::VectorXd O = (t.array()*tt).matrix();

  const double cvk = K.trace();
  // K = S - Saa'S/(a'Sa) deflated S
  Eigen::MatrixXd L = t * O.transpose();
  K = K - t * O.transpose(); //deflated S
  vexp =  cvk - K.trace() ;

  // deflate D

  // N = aa'S/(tt) = a*t'/(tt) = a*O' (n x p)
  Eigen::MatrixXd N = Eigen::MatrixXd::Zero(n, p);
  for (int i = 0; i < n; i++)
    for (int j = 0; j < p; j++)
      N(i, j) += a(i) * O(j);

  //M = Daa'S/(a'Sa) = D.transpose() * N; // (p, p)
  Eigen::MatrixXd M = Eigen::MatrixXd::Zero(p,p);
  for (int i = 0; i < p; i++)
    for (int j = 0; j < p; j++)
      for(int k = 0; k < n; k++)
        M(i, j) += D(i, ind(k)) * N(k, j);  // (p, p)

  //  H = N.transpose() * M (p x p)
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(p,p); //
  for (int i = 0; i < p; i++)
    for (int j = 0; j < p; j++)
      for(int k = 0; k < n; k++)
        H(i, j) += N(k, i) * M(ind(k), j);
  D = (D.array() - M.array() - M.transpose().array() + H.array()).matrix();
  return;
}
//

// finds max part corr exclude small ss, pdates indnot returns ind
int findmax(Eigen::VectorXi& indnot, Eigen::VectorXd vt){

  double p = indnot.size();
  double m = 0.0;
  int ind = 0;
  for (int i = 0; i < p; i++){
    if (indnot(i) == -2){
      if(vt(i) > m){
        m = vt(i);
        ind = i;
      }
    }
  }
  indnot(ind) = ind;
  return ind;
}

// fixed
void fwd_selectC(Eigen::MatrixXd S, Eigen::VectorXi& ind, int& card,
                 Eigen::VectorXd si, double totvexp, double pvexp,
                 double fullrank = 0.0){
  Eigen::VectorXd sik = si;
  int p = S.cols();
  // int induno;
  double tmp;
  Eigen::VectorXd vexpt(p);
  Eigen::VectorXd cvexpt(p);
  Eigen::VectorXd vt(p);
  Eigen::VectorXi indnot = Eigen::VectorXi::Constant(p, -2);
  Eigen::VectorXd ba(p);

  for (int i=0; i < p; i++)
    vt(i) = sik(i) * sik(i) / S(i,i);

  ind(0) = findmax(indnot, vt);

  vexpt(0) = vt(ind(0));
  cvexpt(0) = vt(ind(0));
  int i = 1;
  bool stopSelect = false;
  // start looping ============================================
  while (stopSelect == false){

    tmp = sik(ind(i - 1))/S(ind(i - 1), ind(i - 1));
    for (int j = 0; j < p; j++){
      if ( indnot(j) == -2){
        sik(j) = sik(j) -  (tmp * S(ind(i-1), j));
      }
      else{
        sik(j) = 0;
      }
    }

    ba = (S.col(ind(i-1)).array()/sqrt(S(ind(i-1), ind(i-1)))).matrix();
    S = S - ba * ba.transpose();

    for (int j = 0; j < p; j++){
      if ( indnot(j) == -2){
        if (S(j,j)> fullrank)
          vt(j) = sik(j) * sik(j)/S(j,j);
        else{
          indnot(j) = -1;
          vt(j) = 0;
        }
      }
      else{
        vt(j) = 0;
      }
    }

    ind(i) = findmax(indnot, vt);
    indnot(ind(i)) = 0;

    vexpt(i) =  vt(ind(i));
    cvexpt(i) = cvexpt(i-1) + vexpt(i);

    if (cvexpt(i) >= pvexp*totvexp){
      card = i + 1;
      stopSelect = true;
    }
    else{
      i = i + 1;
    }
    //    Rcpp::checkUserInterrupt();

  }
}

/* non serve
// power method computes only first eigvec, about 82 times faster tha eigen!
Eigen::VectorXd eigvecPMC(Eigen::MatrixXd& X, double& val, double eps = 10E-5){
  const int p = X.cols();
  double sqp = sqrt(double(p));
  Eigen::VectorXd v0 = VectorXd::Constant(p, 1.0/sqp);
  Eigen::VectorXd v = VectorXd::Constant(p, 0.0);
  double stp = 1.0;
  int k = 0;
  while (stp > eps){
    v = X * v0;
    val = v.norm();
    v = v.array()/val;
    stp = (v0.array() - v.array()).matrix().norm();
    v0 = v;
    k++;
    if (k > 100){
      Rf_warning("Powermethod: not converged in 100 iterations. Error is", k);
      break;//here should use try-catch
    }
  }
  //  Rcout << "k = " << k << "; stp = " << stp << endl;
  return (v.array() * val);
}

*/

// This is the main function for R
// S correl matrix
// pvexpfs is proportion of PC to explain by each block
// pvexp is proportion total variance of matrix to explain to terminate computing comps
// ncomps nistead of pvexp maximum number of comps (priority)
// full rank small eps to discard vars from selection
// newpc if false uses PCs of S not compute newpc each block, for large mats
// pass D
// [[Rcpp::export]]
List fspcaCD(Eigen::MatrixXd S, Eigen::MatrixXd D, double pvexpfs = 0.95, double pvexp = 0.95,
               int ncomps = 0, double fullrank = 0, bool newpc = true, double eps = 10E-8){
  int p = S.cols();
  if (ncomps == 0)
    ncomps = p;
  Eigen::MatrixXd K(S);
  Eigen::MatrixXd M = D;

  SelfAdjointEigenSolver<Eigen::MatrixXd> es(S);
  // here could compute D as   vec * diag(val^2) * vec.transpose

  Eigen::MatrixXd vec  = es.eigenvectors().rowwise().reverse();
  Eigen::VectorXd vexppc = es.eigenvalues().reverse();

  double totvexp = vexppc.sum();// total variance S
  double maxvexp = vexppc(0);// this is vexp by first PC for fow_select

  Eigen::VectorXd si = vec.col(0) * vexppc(0);

  Eigen::VectorXd a(p);
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(p, ncomps);
  //  List load(p);
  List indout(p);

  Eigen::VectorXd vexp = Eigen::VectorXd::Zero(ncomps);
  Eigen::VectorXd cvexp = vexp;
  double cvt;
  Eigen::VectorXi indj(p);//this to pass to fwd_select

  Eigen::MatrixXd Sd(p, p);// this takes S[onlyind, onlyind]
  Eigen::MatrixXd Dd(p, p);// this takes D[onlyind,onlyind] deflated

  int cardt = 0;
  int totcard = 0;
  Eigen::VectorXi card(p);
  int nc = 0;
  bool stopComp = false;

  int j = 0;
  while (stopComp == false){
    fwd_selectC(S, indj, cardt, si, maxvexp, pvexpfs, fullrank);

    card(j) = cardt;
    std::sort(indj.data(),indj.data() + cardt);

    // if ( j == 2)
    //   Rf_error("done 1");

    totcard = totcard + cardt;// a che serve ?

    // create submatrices for computing loaidngs
    Sd.topLeftCorner(cardt, cardt) = makeSubS(S, indj.head(cardt));
    Dd.topLeftCorner(cardt, cardt) = makeSubS(M, indj.head(cardt));

    //  compute loadings
    GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(Dd.topLeftCorner(cardt, cardt),
                                                          Sd.topLeftCorner(cardt, cardt));
    // save loadings
    a.head(cardt) = es.eigenvectors().col(cardt - 1);
    // save loadings in column j
    for (int i = 0; i < cardt; i++){
      A(indj(i), j) = es.eigenvectors()(i, cardt - 1);
    }
    // save loadings in list
    indout[j] = indj.head(cardt).array() + 1;

    nc = nc + 1;

    // this new func deflates S and M using only last vector of loads
    // returns deflated matr by references and vexp (not cum vexp)
    deflSandDC(a.head(cardt), K, M, indj.head(cardt), cvt);

    vexp(j) = cvt;
    if (j > 0)
      cvexp(j) = cvt + cvexp(j-1);
    else
      cvexp(j) = cvt;

    // checks if stopComp met
    if ((cvexp(j) > pvexp * totvexp) || ((j + 1) == ncomps)){
      stopComp = true;
      ncomps = nc;
    }
    else{
      if (newpc == true){
        SelfAdjointEigenSolver<Eigen::MatrixXd> es(K);
        // // this is ok because X'K = K'K, so X'Kv = K'Kv = v*lambda_1
        maxvexp =  es.eigenvalues()(p-1);
        si = es.eigenvectors().col(p-1).array() * maxvexp;
        // this power method, returns si and passes maxvexp byref
        //si = eigvecPMC(K, maxvexp, eps);
      }
      else{// this takes the jth pc
        maxvexp =  vexppc(j);
        si = vec.col(j).array() * maxvexp;
      }
      j = j + 1;
    }
  }//end compute comps

  IntegerVector idx = Rcpp::seq(0, nc - 1);

  return  List::create(Named("loadings") = A.topLeftCorner(p,nc), Named("ncomps") = nc,
                       Named("ind") = indout[idx], Named("card") = card.head(nc),
                       Named("vexp") = vexp.head(nc), Named("cvexp") = cvexp.head(nc));
}


#include <iostream>
#include <armadillo>
#include <list>

#include "element.h"
#include "assembly.h"
#include "boundary.h"

using namespace std;
using namespace arma;

//TODO create material.h
//TODO parse input file to initialize model data
//TODO parse command line args
//TODO it may make more sense to restructure data, each element containing
//  pointers to node objects?

int main(int argc, char** argv)
{
  cout << "Program launched." << endl;
  int i;

  /* initialize model data */
  const double h = 0.1;
  const long long int E = 200e9;
  const double v = 0.3;
  const int rho = 7800;
  const double g = 9.8;

  /* body and traction forces */
  vec b(Q4__DOF_PER_NODE);
  b     << 0            << endr
        << -(rho * g)   << endr;
  vec t(Q4__DOF_PER_NODE);
  t     << 0            << endr
        << 0            << endr;

  /* boundary conditions */
  list<BC*> bounds;
  bounds.push_front(new BC(0, 0));
  bounds.push_front(new BC(1, 0));
  bounds.push_front(new BC(3, 0));
  bounds.push_front(new BC(5, 0));

  /* mesh */
  cout << "Creating mesh..." << endl;

  const int numElem = 2;
  const int numNodes = 6;

  int gnodes[] = {1, 2, 5, 4};
  mat gcoords(Q4__NUM_NODES, Q4__DOF_PER_NODE);
  gcoords   << 0.    << 0.    << endr
            << 10.   << 2.    << endr
            << 5.    << 8.    << endr
            << 2.    << 6.    << endr;
  int gdofs[] = {1, 2, 3, 4, 9, 10, 7, 8};
  Q4 *elem = new Q4(E, v, h, &b, &t, gnodes, &gcoords, gdofs);

  int gnodes2[] = {2, 3, 6, 5};
  mat gcoords2(Q4__NUM_NODES, Q4__DOF_PER_NODE);
  gcoords2  << 10.   << 2.    << endr
            << 20.   << 0.    << endr
            << 17.   << 6.    << endr
            << 5.    << 8.    << endr;
  int gdof2[] = {3, 4, 5, 6, 11, 12, 9, 10};
  Q4 *elem2 = new Q4(E, v, h, &b, &t, gnodes2, &gcoords2, gdof2);

  /* calculate element stiffnesses and assemble */
  cout << "Analyzing discretized system..." << endl;

  MechElem *pelems[numElem] = {elem, elem2};

  mat kg = zeros<mat>(Q4__DOF_PER_NODE*numNodes, Q4__DOF_PER_NODE*numNodes);
  mglobalStiffness(kg, pelems, numElem, Q4__DOF_PER_NODE*Q4__NUM_NODES, PSTRESS);

  cout << endl << "Global Stiffness Matrix" << endl
       << kg << endl;

  /* calculate element body forces and assemble */
  vec bg(Q4__DOF_PER_NODE*numNodes);
  mglobalBodyForce(bg, pelems, numElem, Q4__DOF_PER_NODE*Q4__NUM_NODES);

  cout << endl << "Global Body Force" << endl
       << bg << endl;

  /* assemble global force vector */
  vec fg = zeros<vec>(Q4__DOF_PER_NODE*numNodes);
  fg(4) = 10.e3;
  fg(7) = -10.e3;
  fg(9) = -10.e3;
  for (i = 0; i < Q4__DOF_PER_NODE*numNodes; i++)
    fg(i) += bg(i);

  cout << endl << "Global Force" << endl
       << fg << endl;

  /* impose boundary condtions */
  cout << endl << "Imposing boundary conditions..." << endl;
  mimposeBoundaryConds(kg, fg, bounds);

  /* solve for displacements */
  cout << "Solving for displacements..." << endl;
  vec ug = zeros<vec>(Q4__DOF_PER_NODE*numNodes);
  if (!solve(ug, kg, fg))
  {
    cout << endl << "Error: solution not found" << endl;
    return 1;
  }

  /* display answer */
  cout << endl << "================================" << endl;
  cout << endl << "Modified Global Stiffness Matrix (by imposing EBCs)" << endl
       << kg << endl;
  cout << endl << "Modified Global Force (by imposing EBCs)" << endl
       << fg << endl;
  cout << endl << "Global Displacements" << endl
       << ug << endl;

  /* clean up dynamic memory */
  cout << endl << "Cleaning up allocated memory..." << endl;
  for (BC *pbc : bounds) delete pbc;
  delete elem;
  delete elem2;

  return 0;
}


/**
 * @file
 * @brief NPDE homework ProjectionOntoGradients code
 * @author ?, Philippe Peter
 * @date December 2019
 * @copyright Developed at ETH Zurich
 */

#include "../projectionontogradients.h"

#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <memory>

namespace ProjectionOntoGradients::test {

TEST(ProjectionOntoGradients, ElementMatrixProvider) {
  // Building triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);

  // implemented element matrix provider
  ElementMatrixProvider my_elem_mat_provider{};

  // For comparison
  lf::uscalfe::LinearFELaplaceElementMatrix lfe_elem_mat_provider{};

  // loop over cells and compute element matrices
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    Eigen::Matrix3d my_mat{my_elem_mat_provider.Eval(*cell)};
    lf::uscalfe::LinearFELaplaceElementMatrix::ElemMat lfe_mat{
        lfe_elem_mat_provider.Eval(*cell)};

    // compare element matrices:
    EXPECT_NEAR((lfe_mat.block<3, 3>(0, 0) - my_mat).norm(), 0.0, 1E-3);
  }
}

TEST(ProjectionOntoGradients, GradProjRhsProvider_1) {
  // Building triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);

  // initialize functions f and v for which the linear form is evaluated
  auto f = [](Eigen::Vector2d x) { return Eigen::Vector2d(1.0, 2.0); };
  auto v = [](Eigen::Vector2d x) { return 2 * x(0) + x(1); };
  auto v_mf = lf::mesh::utils::MeshFunctionGlobal(v);

  // set up finite elements
  std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>> fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // construct element vector provider
  GradProjRhsProvider my_vec_provider(f);

  // assemble vector
  Eigen::VectorXd phi(fe_space->LocGlobMap().NumDofs());
  phi.setZero();
  lf::assemble::AssembleVectorLocally(0, fe_space->LocGlobMap(),
                                      my_vec_provider, phi);

  // project v onto the fe space
  auto v_vec = lf::fe::NodalProjection<double>(*fe_space, v_mf);

  // evaluate linear form on projected function:
  auto product = (v_vec.transpose() * phi).eval();
  EXPECT_NEAR(product(0, 0), 36, 1E-5);
}

TEST(ProjectionOntoGradients, GradProjRhsProvider_2) {
  // Building triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);

  // initialize functions f and v for which the linear form is evaluated
  auto f = [](Eigen::Vector2d x) { return Eigen::Vector2d(x(1), x(0)); };
  auto v = [](Eigen::Vector2d x) { return 2 * x(0) + x(1); };
  auto v_mf = lf::mesh::utils::MeshFunctionGlobal(v);

  // set up finite elements
  std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>> fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // construct element vector provider
  GradProjRhsProvider my_vec_provider(f);

  // assemble vector
  Eigen::VectorXd phi(fe_space->LocGlobMap().NumDofs());
  phi.setZero();
  lf::assemble::AssembleVectorLocally(0, fe_space->LocGlobMap(),
                                      my_vec_provider, phi);

  // project v onto the fe space
  auto v_vec = lf::fe::NodalProjection<double>(*fe_space, v_mf);

  // evaluate linear form on projected function:
  auto product = (v_vec.transpose() * phi).eval();
  EXPECT_NEAR(product(0, 0), 40.5, 1E-5);
}

/* SAM_LISTING_BEGIN_1 */
TEST(ProjectionOntoGradients, div_free_test) {
  // Building test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  // Divergence-free vector field
  const auto f = [](Eigen::Vector2d x) { return Eigen::Vector2d(-x(1), x(0)); };

  // DofHandler for the p.w. linear Lagrangian finite element
  lf::assemble::UniformFEDofHandler dofh(mesh_p,
                                         {{lf::base::RefEl::kPoint(), 1},
                                          {lf::base::RefEl::kSegment(), 0},
                                          {lf::base::RefEl::kTria(), 0}});
  // Compute solution
  const Eigen::VectorXd sol_vec =
      ProjectionOntoGradients::projectOntoGradients(dofh, f);

  // As stated in the exercise, we would expect the solution to be zero.
  // Hence we check every entry if its (numerically) zero.
  // The GoogleTest framework provides a function we may use:
  /*   EXPECT_NEAR(value 1, value 2, max. difference) */
  const double eps = 1e-15;
  for (std::size_t i = 0; i < sol_vec.size(); ++i) {
    EXPECT_NEAR(sol_vec[i], 0.0, eps);
    // Try testing for equality, you'll see it will fail miserably!
    /* EXPECT_EQ(sol_vec[i], 0.0); */
  }
}
/* SAM_LISTING_END_1 */

TEST(ProjectionOntoGradients, exact_sol_test) {
  // I. Construct the test mesh
  // mesh builder in a world of dimension 2
  lf::mesh::utils::TPTriagMeshBuilder my_builder(
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));

  // define the test mesh
  my_builder.setBottomLeftCorner(Eigen::Vector2d{0, 0})
      .setTopRightCorner(Eigen::Vector2d{1, 1})
      .setNumXCells(10)
      .setNumYCells(10);

  auto mesh_p = my_builder.Build();

  // II. Construct a linear finite element space on the test mesh
  lf::uscalfe::FeSpaceLagrangeO1<double> fe_space(mesh_p);

  // III. Define a function which computes the index of the triangle in which
  // the coorindates of a given point are
  const auto triangleIndex = [](Eigen::Vector2d x) {
    if (x(0) >= 0.0 && x(0) <= 0.5 && x(1) >= 0.0 && x(1) <= 0.5) {
      if (x(0) < x(1))
        return 0;
      else
        return 1;
    } else if (x(0) >= 0.0 && x(0) <= 0.5 && x(1) >= 0.5 && x(1) <= 1.0) {
      if (x(0) + 0.5 < x(1))
        return 2;
      else
        return 3;
    } else if (x(0) >= 0.5 && x(0) <= 1.0 && x(1) >= 0.0 && x(1) <= 0.5) {
      if (x(0) < 0.5 + x(1))
        return 4;
      else
        return 5;
    } else if (x(0) >= 0.5 && x(0) <= 1.0 && x(1) >= 0.5 && x(1) <= 1.0) {
      if (x(0) < x(1))
        return 6;
      else
        return 7;
    } else {
      LF_ASSERT_MSG(false, "Coordinates outside of unit square");
    }
  };

  // IV. Define the function f
  const auto f = [&triangleIndex](Eigen::Vector2d x) {
    int triang_idx = triangleIndex(x);
    // return the function value according to the definition in the
    // exercise
    switch (triang_idx) {
      case 0:
        return Eigen::Vector2d(2, 0);
      case 1:
        return Eigen::Vector2d(0, 2);
      case 3:
        return Eigen::Vector2d(2, -2);
      case 4:
        return Eigen::Vector2d(-2, 2);
      case 6:
        return Eigen::Vector2d(0, -2);
      case 7:
        return Eigen::Vector2d(-2, 0);
      default:
        return Eigen::Vector2d(0, 0);
    }
  };

  // V. Define the tent function associated with the central node
  const auto tentFunction = [&triangleIndex](Eigen::Vector2d x) {
    int triang_idx = triangleIndex(x);
    // Observe that the restriction of the tent function on
    // any adjecant triangle is by definition a linear function a*x + b*y + c.
    // The gradients of these linear functions are given in the exercise
    // so we can read of the values of a and b. We then compute the
    // value of c to ensure that the linear function evaluates to
    // one at the central node.
    switch (triang_idx) {
      case 0:
        return 2.0 * x(0);
      case 1:
        return 2.0 * x(1);
      case 3:
        return 2.0 * x(0) - 2.0 * x(1) + 1.0;
      case 4:
        return -2.0 * x(0) + 2.0 * x(1) + 1.0;
      case 6:
        return -2.0 * x(1) + 2.0;
      case 7:
        return -2.0 * x(0) + 2.0;
      default:
        return 0.0;
    }
  };

  // VI.Determine the coefficient vector of the tent function in
  // the FE space (perform a Nodal projection)
  // The Function lf::fe::NodalProjection requires a mesh function as its
  // second argument, so we first construct a meshFunction object which
  // describes the tent function.
  auto tentFunction_mf = lf::mesh::utils::MeshFunctionGlobal(tentFunction);
  auto ref_vec = lf::fe::NodalProjection<double>(fe_space, tentFunction_mf);

  // VII. Test your implementation
  const Eigen::VectorXd sol_vec =
      projectOntoGradients(fe_space.LocGlobMap(), f);
  EXPECT_NEAR((sol_vec - ref_vec).norm(), 0.0, 1e-12);
}

}  // namespace ProjectionOntoGradients::test


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file CombinedImuFactorsExample
 * @brief Test example for using GTSAM ImuCombinedFactor
 * navigation code.
 * @author Varun Agrawal
 */

/**
 * Example of use of the CombinedImuFactor in
 * conjunction with GPS
 *  - we read IMU and GPS data from a CSV file, with the following format:
 *  A row starting with "i" is the first initial position formatted with
 *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
 *  A row starting with "0" is an imu measurement
 *  (body frame - Forward, Right, Down)
 *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
 *  A row starting with "1" is a gps correction formatted with
 *  N, E, D, qX, qY, qZ, qW
 * Note that for GPS correction, we're only using the position not the
 * rotation. The rotation is provided in the file for ground truth comparison.
 *
 *  See usage: ./CombinedImuFactorsExample --help
 */

#include <boost/program_options.hpp>

// GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>

#include <cstring>
#include <fstream>
#include <iostream>

using namespace gtsam;
using namespace std;

using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)

namespace po = boost::program_options;

po::variables_map parseOptions(int argc, char* argv[]) {
  po::options_description desc;
  desc.add_options()("help,h", "produce help message")(
      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
      "path to the CSV file with the IMU data")(
      "output_filename",
      po::value<string>()->default_value("imuFactorExampleResults.csv"),
      "path to the result file to use")("use_isam", po::bool_switch(),
                                        "use ISAM as the optimizer");

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

  if (vm.count("help")) {
    cout << desc << "\n";
    exit(1);
  }

  return vm;
}

Vector10 readInitialState(ifstream& file) {
  string value;
  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
  Vector10 initial_state;
  getline(file, value, ',');  // i
  for (int i = 0; i < 9; i++) {
    getline(file, value, ',');
    initial_state(i) = stof(value.c_str());
  }
  getline(file, value, '\n');
  initial_state(9) = stof(value.c_str());

  return initial_state;
}

boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
  // We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924;
  double gyro_noise_sigma = 0.000205689024915;
  double accel_bias_rw_sigma = 0.004905;
  double gyro_bias_rw_sigma = 0.000001454441043;
  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
  Matrix33 integration_error_cov =
      I_3x3 * 1e-8;  // error committed in integrating position from velocities
  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
  Matrix66 bias_acc_omega_int =
      I_6x6 * 1e-5;  // error in the bias used for preintegration

  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
  // PreintegrationBase params:
  p->accelerometerCovariance =
      measured_acc_cov;  // acc white noise in continuous
  p->integrationCovariance =
      integration_error_cov;  // integration uncertainty continuous
  // should be using 2nd order integration
  // PreintegratedRotation params:
  p->gyroscopeCovariance =
      measured_omega_cov;  // gyro white noise in continuous
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
  p->biasAccOmegaInt = bias_acc_omega_int;

  return p;
}

int main(int argc, char* argv[]) {
  string data_filename, output_filename;
  po::variables_map var_map = parseOptions(argc, argv);

  data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
  output_filename = var_map["output_filename"].as<string>();

  // Set up output file for plotting errors
  FILE* fp_out = fopen(output_filename.c_str(), "w+");
  fprintf(fp_out,
          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
          "gt_qy,gt_qz,gt_qw\n");

  // Begin parsing the CSV file.  Input the first line for initialization.
  // From there, we'll iterate through the file and we'll preintegrate the IMU
  // or add in the GPS given the input.
  ifstream file(data_filename.c_str());

  Vector10 initial_state = readInitialState(file);
  cout << "initial state:\n" << initial_state.transpose() << "\n\n";

  // Assemble initial quaternion through GTSAM constructor
  // ::Quaternion(w,x,y,z);
  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                         initial_state(4), initial_state(5));
  Point3 prior_point(initial_state.head<3>());
  Pose3 prior_pose(prior_rotation, prior_point);
  Vector3 prior_velocity(initial_state.tail<3>());

  imuBias::ConstantBias prior_imu_bias;  // assume zero initial bias

  int index = 0;

  Values initial_values;

  // insert pose at initialization
  initial_values.insert(X(index), prior_pose);
  initial_values.insert(V(index), prior_velocity);
  initial_values.insert(B(index), prior_imu_bias);

  // Assemble prior noise model and add it the graph.`
  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
      (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
          .finished());  // rad,rad,rad,m, m, m
  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);

  // Add all prior factors (pose, velocity, bias) to the graph.
  NonlinearFactorGraph graph;
  graph.addPrior<Pose3>(X(index), prior_pose, pose_noise_model);
  graph.addPrior<Vector3>(V(index), prior_velocity, velocity_noise_model);
  graph.addPrior<imuBias::ConstantBias>(B(index), prior_imu_bias,
                                        bias_noise_model);

  auto p = imuParams();

  std::shared_ptr<PreintegrationType> preintegrated =
      std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);

  assert(preintegrated);

  // Store previous state for imu integration and latest predicted outcome.
  NavState prev_state(prior_pose, prior_velocity);
  NavState prop_state = prev_state;
  imuBias::ConstantBias prev_bias = prior_imu_bias;

  // Keep track of total error over the entire run as simple performance metric.
  double current_position_error = 0.0, current_orientation_error = 0.0;

  double output_time = 0.0;
  double dt = 0.005;  // The real system has noise, but here, results are nearly
                      // exactly the same, so keeping this for simplicity.

  // All priors have been set up, now iterate through the data file.
  while (file.good()) {
    // Parse out first value
    string value;
    getline(file, value, ',');
    int type = stoi(value.c_str());

    if (type == 0) {  // IMU measurement
      Vector6 imu;
      for (int i = 0; i < 5; ++i) {
        getline(file, value, ',');
        imu(i) = stof(value.c_str());
      }
      getline(file, value, '\n');
      imu(5) = stof(value.c_str());

      // Adding the IMU preintegration.
      preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);

    } else if (type == 1) {  // GPS measurement
      Vector7 gps;
      for (int i = 0; i < 6; ++i) {
        getline(file, value, ',');
        gps(i) = stof(value.c_str());
      }
      getline(file, value, '\n');
      gps(6) = stof(value.c_str());

      index++;

      // Adding IMU factor and GPS factor and optimizing.
      auto preint_imu_combined =
          dynamic_cast<const PreintegratedCombinedMeasurements&>(
              *preintegrated);
      CombinedImuFactor imu_factor(X(index - 1), V(index - 1), X(index),
                                   V(index), B(index - 1), B(index),
                                   preint_imu_combined);
      graph.add(imu_factor);

      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
      GPSFactor gps_factor(X(index),
                           Point3(gps(0),   // N,
                                  gps(1),   // E,
                                  gps(2)),  // D,
                           correction_noise);
      graph.add(gps_factor);

      // Now optimize and compare results.
      prop_state = preintegrated->predict(prev_state, prev_bias);
      initial_values.insert(X(index), prop_state.pose());
      initial_values.insert(V(index), prop_state.v());
      initial_values.insert(B(index), prev_bias);

      LevenbergMarquardtParams params;
      params.setVerbosityLM("SUMMARY");
      LevenbergMarquardtOptimizer optimizer(graph, initial_values, params);
      Values result = optimizer.optimize();

      // Overwrite the beginning of the preintegration for the next step.
      prev_state =
          NavState(result.at<Pose3>(X(index)), result.at<Vector3>(V(index)));
      prev_bias = result.at<imuBias::ConstantBias>(B(index));

      // Reset the preintegration object.
      preintegrated->resetIntegrationAndSetBias(prev_bias);

      // Print out the position and orientation error for comparison.
      Vector3 result_position = prev_state.pose().translation();
      Vector3 position_error = result_position - gps.head<3>();
      current_position_error = position_error.norm();

      Quaternion result_quat = prev_state.pose().rotation().toQuaternion();
      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
      Quaternion quat_error = result_quat * gps_quat.inverse();
      quat_error.normalize();
      Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
                                quat_error.z() * 2);
      current_orientation_error = euler_angle_error.norm();

      // display statistics
      cout << "Position error:" << current_position_error << "\t "
           << "Angular error:" << current_orientation_error << "\n"
           << endl;

      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
              output_time, result_position(0), result_position(1),
              result_position(2), result_quat.x(), result_quat.y(),
              result_quat.z(), result_quat.w(), gps(0), gps(1), gps(2),
              gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());

      output_time += 1.0;

    } else {
      cerr << "ERROR parsing file\n";
      return 1;
    }
  }
  fclose(fp_out);
  cout << "Complete, results written to " << output_filename << "\n\n";

  return 0;
}


#include <stan/math/mix/mat.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <vector>
#include <limits>

using stan::math::fvar;
using stan::math::var;

TEST(AgradMixMatrixSquaredDistance, vector_fv_vector_fv1) {
  stan::math::vector_fv v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;

  stan::math::fvar<var> a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(v1(0).val_);
  vars.push_back(v1(1).val_);
  vars.push_back(v1(2).val_);
  vars.push_back(v2(0).val_);
  vars.push_back(v2(1).val_);
  vars.push_back(v2(2).val_);

  a.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(10, grads[1]);
  EXPECT_FLOAT_EQ(-8, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(-10, grads[4]);
  EXPECT_FLOAT_EQ(8, grads[5]);

  v1.resize(0);
  v2.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v1, v2).val_.val());

  v1.resize(1);
  v2.resize(2);
  v1 << 1;
  v2 << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v1, v2), std::invalid_argument);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_vector_fv2) {
  stan::math::vector_fv v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;

  stan::math::fvar<var> a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(v1(0).val_);
  vars.push_back(v1(1).val_);
  vars.push_back(v1(2).val_);
  vars.push_back(v2(0).val_);
  vars.push_back(v2(1).val_);
  vars.push_back(v2(2).val_);

  a.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}

TEST(AgradMixMatrixSquaredDistance, rowvector_fv_vector_fv1) {
  stan::math::row_vector_fv rv;
  stan::math::vector_fv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<var> a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_);
  vars.push_back(rv(1).val_);
  vars.push_back(rv(2).val_);
  vars.push_back(v(0).val_);
  vars.push_back(v(1).val_);
  vars.push_back(v(2).val_);

  a.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(10, grads[1]);
  EXPECT_FLOAT_EQ(-8, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(-10, grads[4]);
  EXPECT_FLOAT_EQ(8, grads[5]);

  rv.resize(0);
  v.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(rv, v).val_.val());

  rv.resize(1);
  v.resize(2);
  rv << 1;
  v << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(rv, v), std::invalid_argument);
}

TEST(AgradMixMatrixSquaredDistance, rowvector_fv_vector_fv2) {
  stan::math::row_vector_fv rv;
  stan::math::vector_fv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<var> a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_);
  vars.push_back(rv(1).val_);
  vars.push_back(rv(2).val_);
  vars.push_back(v(0).val_);
  vars.push_back(v(1).val_);
  vars.push_back(v(2).val_);

  a.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_rowvector_fv1) {
  stan::math::row_vector_fv rv;
  stan::math::vector_fv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<var> a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_);
  vars.push_back(rv(1).val_);
  vars.push_back(rv(2).val_);
  vars.push_back(v(0).val_);
  vars.push_back(v(1).val_);
  vars.push_back(v(2).val_);

  a.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(10, grads[1]);
  EXPECT_FLOAT_EQ(-8, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(-10, grads[4]);
  EXPECT_FLOAT_EQ(8, grads[5]);

  v.resize(0);
  rv.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v, rv).val_.val());

  v.resize(1);
  rv.resize(2);
  v << 1;
  rv << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v, rv), std::invalid_argument);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_rowvector_fv2) {
  stan::math::row_vector_fv rv;
  stan::math::vector_fv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<var> a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_);
  vars.push_back(rv(1).val_);
  vars.push_back(rv(2).val_);
  vars.push_back(v(0).val_);
  vars.push_back(v(1).val_);
  vars.push_back(v(2).val_);

  a.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}
TEST(AgradMixMatrixSquaredDistance, special_values_fv) {
  stan::math::vector_fv v1, v2;
  v1.resize(1);
  v2.resize(1);

  v1 << 0;
  v2 << std::numeric_limits<double>::quiet_NaN();
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v2, v1)));

  v1 << 0;
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v2, v1)));

  v1 << std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v2, v1)));

  v1 << -std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v2, v1)));
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_vector_ffv1) {
  stan::math::vector_ffv v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(v1(0).val_.val_);
  vars.push_back(v1(1).val_.val_);
  vars.push_back(v1(2).val_.val_);
  vars.push_back(v2(0).val_.val_);
  vars.push_back(v2(1).val_.val_);
  vars.push_back(v2(2).val_.val_);

  a.val_.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(10, grads[1]);
  EXPECT_FLOAT_EQ(-8, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(-10, grads[4]);
  EXPECT_FLOAT_EQ(8, grads[5]);

  v1.resize(0);
  v2.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v1, v2).val_.val_.val());

  v1.resize(1);
  v2.resize(2);
  v1 << 1;
  v2 << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v1, v2), std::invalid_argument);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_vector_ffv2) {
  stan::math::vector_ffv v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;
  v1(0).val_.d_ = 1.0;
  v1(1).val_.d_ = 2.0;
  v1(2).val_.d_ = 3.0;
  v2(0).val_.d_ = 4.0;
  v2(1).val_.d_ = 5.0;
  v2(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(v1(0).val_.val_);
  vars.push_back(v1(1).val_.val_);
  vars.push_back(v1(2).val_.val_);
  vars.push_back(v2(0).val_.val_);
  vars.push_back(v2(1).val_.val_);
  vars.push_back(v2(2).val_.val_);

  a.val_.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_vector_ffv3) {
  stan::math::vector_ffv v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;
  v1(0).val_.d_ = 1.0;
  v1(1).val_.d_ = 2.0;
  v1(2).val_.d_ = 3.0;
  v2(0).val_.d_ = 4.0;
  v2(1).val_.d_ = 5.0;
  v2(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(v1(0).val_.val_);
  vars.push_back(v1(1).val_.val_);
  vars.push_back(v1(2).val_.val_);
  vars.push_back(v2(0).val_.val_);
  vars.push_back(v2(1).val_.val_);
  vars.push_back(v2(2).val_.val_);

  a.d_.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_vector_ffv4) {
  stan::math::vector_ffv v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;
  v1(0).val_.d_ = 1.0;
  v1(1).val_.d_ = 2.0;
  v1(2).val_.d_ = 3.0;
  v2(0).val_.d_ = 4.0;
  v2(1).val_.d_ = 5.0;
  v2(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(v1(0).val_.val_);
  vars.push_back(v1(1).val_.val_);
  vars.push_back(v1(2).val_.val_);
  vars.push_back(v2(0).val_.val_);
  vars.push_back(v2(1).val_.val_);
  vars.push_back(v2(2).val_.val_);

  a.d_.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(0, grads[0]);
  EXPECT_FLOAT_EQ(-0, grads[1]);
  EXPECT_FLOAT_EQ(0, grads[2]);
  EXPECT_FLOAT_EQ(-0, grads[3]);
  EXPECT_FLOAT_EQ(0, grads[4]);
  EXPECT_FLOAT_EQ(-0, grads[5]);
}

TEST(AgradMixMatrixSquaredDistance, rowvector_fv_vector_ffv1) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.val_.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(10, grads[1]);
  EXPECT_FLOAT_EQ(-8, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(-10, grads[4]);
  EXPECT_FLOAT_EQ(8, grads[5]);

  rv.resize(0);
  v.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(rv, v).val_.val_.val());

  rv.resize(1);
  v.resize(2);
  rv << 1;
  v << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(rv, v), std::invalid_argument);
}

TEST(AgradMixMatrixSquaredDistance, rowvector_fv_vector_ffv2) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;
  rv(0).val_.d_ = 1.0;
  rv(1).val_.d_ = 2.0;
  rv(2).val_.d_ = 3.0;
  v(0).val_.d_ = 4.0;
  v(1).val_.d_ = 5.0;
  v(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.val_.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}
TEST(AgradMixMatrixSquaredDistance, rowvector_fv_vector_ffv3) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;
  rv(0).val_.d_ = 1.0;
  rv(1).val_.d_ = 2.0;
  rv(2).val_.d_ = 3.0;
  v(0).val_.d_ = 4.0;
  v(1).val_.d_ = 5.0;
  v(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.d_.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}
TEST(AgradMixMatrixSquaredDistance, rowvector_fv_vector_ffv4) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;
  rv(0).val_.d_ = 1.0;
  rv(1).val_.d_ = 2.0;
  rv(2).val_.d_ = 3.0;
  v(0).val_.d_ = 4.0;
  v(1).val_.d_ = 5.0;
  v(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.d_.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(0, grads[0]);
  EXPECT_FLOAT_EQ(-0, grads[1]);
  EXPECT_FLOAT_EQ(0, grads[2]);
  EXPECT_FLOAT_EQ(-0, grads[3]);
  EXPECT_FLOAT_EQ(0, grads[4]);
  EXPECT_FLOAT_EQ(-0, grads[5]);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_rowvector_ffv1) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.val_.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(10, grads[1]);
  EXPECT_FLOAT_EQ(-8, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(-10, grads[4]);
  EXPECT_FLOAT_EQ(8, grads[5]);

  v.resize(0);
  rv.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v, rv).val_.val_.val());

  v.resize(1);
  rv.resize(2);
  v << 1;
  rv << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v, rv), std::invalid_argument);
}

TEST(AgradMixMatrixSquaredDistance, vector_fv_rowvector_ffv2) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;
  rv(0).val_.d_ = 1.0;
  rv(1).val_.d_ = 2.0;
  rv(2).val_.d_ = 3.0;
  v(0).val_.d_ = 4.0;
  v(1).val_.d_ = 5.0;
  v(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.d_.val_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}
TEST(AgradMixMatrixSquaredDistance, vector_fv_rowvector_ffv3) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;
  rv(0).val_.d_ = 1.0;
  rv(1).val_.d_ = 2.0;
  rv(2).val_.d_ = 3.0;
  v(0).val_.d_ = 4.0;
  v(1).val_.d_ = 5.0;
  v(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.val_.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(-6, grads[0]);
  EXPECT_FLOAT_EQ(-6, grads[1]);
  EXPECT_FLOAT_EQ(-6, grads[2]);
  EXPECT_FLOAT_EQ(6, grads[3]);
  EXPECT_FLOAT_EQ(6, grads[4]);
  EXPECT_FLOAT_EQ(6, grads[5]);
}
TEST(AgradMixMatrixSquaredDistance, vector_fv_rowvector_ffv4) {
  stan::math::row_vector_ffv rv;
  stan::math::vector_ffv v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;
  rv(0).val_.d_ = 1.0;
  rv(1).val_.d_ = 2.0;
  rv(2).val_.d_ = 3.0;
  v(0).val_.d_ = 4.0;
  v(1).val_.d_ = 5.0;
  v(2).val_.d_ = 6.0;

  stan::math::fvar<fvar<var> > a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val_.val());
  EXPECT_FLOAT_EQ(12, a.d_.val_.val());

  std::vector<double> grads;
  std::vector<var> vars;
  vars.push_back(rv(0).val_.val_);
  vars.push_back(rv(1).val_.val_);
  vars.push_back(rv(2).val_.val_);
  vars.push_back(v(0).val_.val_);
  vars.push_back(v(1).val_.val_);
  vars.push_back(v(2).val_.val_);

  a.d_.d_.grad(vars, grads);
  EXPECT_FLOAT_EQ(0, grads[0]);
  EXPECT_FLOAT_EQ(-0, grads[1]);
  EXPECT_FLOAT_EQ(0, grads[2]);
  EXPECT_FLOAT_EQ(-0, grads[3]);
  EXPECT_FLOAT_EQ(0, grads[4]);
  EXPECT_FLOAT_EQ(-0, grads[5]);
}
TEST(AgradMixMatrixSquaredDistance, special_values_ffv) {
  stan::math::vector_ffv v1, v2;
  v1.resize(1);
  v2.resize(1);

  v1 << 0;
  v2 << std::numeric_limits<double>::quiet_NaN();
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v2, v1)));

  v1 << 0;
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v2, v1)));

  v1 << std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_nan(stan::math::squared_distance(v2, v1)));

  v1 << -std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(stan::math::is_inf(stan::math::squared_distance(v2, v1)));
}


#include "remainder.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include "utility/itoa.hpp"
#include <boost/variant.hpp>
#include <cmath>
#include <stdexcept>
#include <algorithm>
namespace HT
{
    void remainder(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=3)
          throw std::runtime_error("Remainder should have exact 2 parameters");
        auto & thirdCh = *astnode->ch.rbegin();
        auto & secondCh = *(++astnode->ch.begin());
        ph.parse(secondCh);
        ph.parse(thirdCh);
        if (secondCh->token.tokenType!=Complex || !boost::get<ComplexType>(secondCh->token.info).isInt())
          throw std::runtime_error("The arguments of Remainder should be integer");

        if (thirdCh->token.tokenType!=Complex || !boost::get<ComplexType>(thirdCh->token.info).isInt())
          throw std::runtime_error("The arguments of Remainder should be integer");

        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto cast =boost::get<ComplexType>(secondCh->token.info);

        if (cast.toInt().isZero()) astnode->token.info = ComplexType(0);
        else
        {
            astnode->token.info = ComplexType(cast.toInt()  \
                        %
                        boost::get<ComplexType>(thirdCh->token.info).toInt()) ;
            if (!cast.exact() || !boost::get<ComplexType>(thirdCh->token.info).exact())
              astnode->token.info = boost::get<ComplexType>(astnode->token.info).toinexact();

            astnode->token.info = ComplexType( boost::get<ComplexType>(astnode->token.info).toInt().setSign(
                            boost::get<ComplexType>(secondCh->token.info).toInt().getSign()));

        }
        astnode->remove();
    }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testGaussianISAM.cpp
 * @brief   Unit tests for GaussianISAM
 * @author  Michael Kaess
 */

#include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/geometry/Rot2.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests

static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
		0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;

static const double tol = 1e-4;

/* ************************************************************************* */
TEST( ISAM, iSAM_smoother )
{
  Ordering ordering;
  for (int t = 1; t <= 7; t++) ordering += X(t);

  // Create smoother with 7 nodes
	GaussianFactorGraph smoother = createSmoother(7, ordering).first;

	// run iSAM for every factor
	GaussianISAM actual;
	BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
		GaussianFactorGraph factorGraph;
		factorGraph.push_back(factor);
		actual.update(factorGraph);
	}

	BayesTree<GaussianConditional>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate();
	// Create expected Bayes Tree by solving smoother with "natural" ordering
	GaussianISAM expected(*bayesTree);

	// Check whether BayesTree is correct
	EXPECT(assert_equal(expected, actual));

	// obtain solution
	VectorValues e(VectorValues::Zero(7,2)); // expected solution
	VectorValues optimized = optimize(actual); // actual solution
	EXPECT(assert_equal(e, optimized));
}

/* ************************************************************************* *
 Bayes tree for smoother with "natural" ordering:
C1 x6 x7
C2   x5 : x6
C3     x4 : x5
C4       x3 : x4
C5         x2 : x3
C6           x1 : x2
**************************************************************************** */
TEST_UNSAFE( BayesTree, linear_smoother_shortcuts )
{
	// Create smoother with 7 nodes
  Ordering ordering;
	GaussianFactorGraph smoother;
	boost::tie(smoother, ordering) = createSmoother(7);

	BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();

	// Create the Bayes tree
	GaussianISAM isamTree(bayesTree);
	LONGS_EQUAL(6,isamTree.size());

	// Check the conditional P(Root|Root)
	GaussianBayesNet empty;
	GaussianISAM::sharedClique R = isamTree.root();
	GaussianBayesNet actual1 = GaussianISAM::shortcut(R,R);
	EXPECT(assert_equal(empty,actual1,tol));

	// Check the conditional P(C2|Root)
	GaussianISAM::sharedClique C2 = isamTree[ordering[X(5)]];
	GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
	EXPECT(assert_equal(empty,actual2,tol));

	// Check the conditional P(C3|Root)
	double sigma3 = 0.61808;
	Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
	GaussianBayesNet expected3;
	push_front(expected3,ordering[X(5)], zero(2), eye(2)/sigma3, ordering[X(6)], A56/sigma3, ones(2));
	GaussianISAM::sharedClique C3 = isamTree[ordering[X(4)]];
	GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
	EXPECT(assert_equal(expected3,actual3,tol));

	// Check the conditional P(C4|Root)
	double sigma4 = 0.661968;
	Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
	GaussianBayesNet expected4;
	push_front(expected4, ordering[X(4)], zero(2), eye(2)/sigma4, ordering[X(6)], A46/sigma4, ones(2));
	GaussianISAM::sharedClique C4 = isamTree[ordering[X(3)]];
	GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R);
	EXPECT(assert_equal(expected4,actual4,tol));
}

/* ************************************************************************* *
 Bayes tree for smoother with "nested dissection" ordering:

	 Node[x1] P(x1 | x2)
	 Node[x3] P(x3 | x2 x4)
	 Node[x5] P(x5 | x4 x6)
	 Node[x7] P(x7 | x6)
	 Node[x2] P(x2 | x4)
	 Node[x6] P(x6 | x4)
	 Node[x4] P(x4)

 becomes

	 C1		 x5 x6 x4
	 C2		  x3 x2 : x4
	 C3		    x1 : x2
	 C4		  x7 : x6

************************************************************************* */
TEST_UNSAFE( BayesTree, balanced_smoother_marginals )
{
  // Create smoother with 7 nodes
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianFactorGraph smoother = createSmoother(7, ordering).first;

  // Create the Bayes tree
  BayesTree<GaussianConditional> chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate();

	VectorValues expectedSolution(VectorValues::Zero(7,2));
	VectorValues actualSolution = optimize(chordalBayesNet);
	EXPECT(assert_equal(expectedSolution,actualSolution,tol));

	// Create the Bayes tree
	GaussianISAM bayesTree(chordalBayesNet);
	LONGS_EQUAL(4,bayesTree.size());

	double tol=1e-5;

	// Check marginal on x1
	GaussianBayesNet expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1);
	GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[X(1)]);
	Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
	Matrix actualCovarianceX1;
	actualCovarianceX1 = bayesTree.marginalCovariance(ordering[X(1)]);
	EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
	EXPECT(assert_equal(expected1,actual1,tol));

	// Check marginal on x2
	double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
	GaussianBayesNet expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2);
	GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[X(2)]);
	Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2);
	Matrix actualCovarianceX2;
	actualCovarianceX2 = bayesTree.marginalCovariance(ordering[X(2)]);
	EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol));
	EXPECT(assert_equal(expected2,actual2,tol));

	// Check marginal on x3
	GaussianBayesNet expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3);
	GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[X(3)]);
	Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3);
	Matrix actualCovarianceX3;
	actualCovarianceX3 = bayesTree.marginalCovariance(ordering[X(3)]);
	EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol));
	EXPECT(assert_equal(expected3,actual3,tol));

	// Check marginal on x4
	GaussianBayesNet expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4);
	GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[X(4)]);
	Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4);
	Matrix actualCovarianceX4;
	actualCovarianceX4 = bayesTree.marginalCovariance(ordering[X(4)]);
	EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol));
	EXPECT(assert_equal(expected4,actual4,tol));

	// Check marginal on x7 (should be equal to x1)
	GaussianBayesNet expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7);
	GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[X(7)]);
	Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7);
	Matrix actualCovarianceX7;
	actualCovarianceX7 = bayesTree.marginalCovariance(ordering[X(7)]);
	EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol));
	EXPECT(assert_equal(expected7,actual7,tol));
}

/* ************************************************************************* */
TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
{
	// Create smoother with 7 nodes
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
	GaussianFactorGraph smoother = createSmoother(7, ordering).first;

	// Create the Bayes tree
	BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
	GaussianISAM isamTree(bayesTree);

	// Check the conditional P(Root|Root)
	GaussianBayesNet empty;
	GaussianISAM::sharedClique R = isamTree.root();
	GaussianBayesNet actual1 = GaussianISAM::shortcut(R,R);
	EXPECT(assert_equal(empty,actual1,tol));

	// Check the conditional P(C2|Root)
	GaussianISAM::sharedClique C2 = isamTree[ordering[X(3)]];
	GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R);
	EXPECT(assert_equal(empty,actual2,tol));

	// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
	/** TODO: Note for multifrontal conditional:
	 * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
	 * We don't know yet how to take it out.
	 */
//	GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
//	p_x2_x4->print("Conditional p_x2_x4: ");
//	GaussianBayesNet expected3(p_x2_x4);
//	GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
//	GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
//	EXPECT(assert_equal(expected3,actual3,tol));
}

///* ************************************************************************* */
//TEST( BayesTree, balanced_smoother_clique_marginals )
//{
//  // Create smoother with 7 nodes
//  Ordering ordering;
//  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
//  GaussianFactorGraph smoother = createSmoother(7, ordering).first;
//
//  // Create the Bayes tree
//  GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
//  GaussianISAM bayesTree(chordalBayesNet);
//
//	// Check the clique marginal P(C3)
//	double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
//	GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt);
//	push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
//	GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
//	GaussianFactorGraph marginal = C3->marginal(R);
//	GaussianVariableIndex varIndex(marginal);
//	Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
//	Permutation toFrontInverse(*toFront.inverse());
//	varIndex.permute(toFront);
//	BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
//	  factor->permuteWithInverse(toFrontInverse); }
//	GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
//	actual.permuteWithInverse(toFront);
//	EXPECT(assert_equal(expected,actual,tol));
//}

/* ************************************************************************* */
TEST_UNSAFE( BayesTree, balanced_smoother_joint )
{
	// Create smoother with 7 nodes
	Ordering ordering;
	ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
	GaussianFactorGraph smoother = createSmoother(7, ordering).first;

	// Create the Bayes tree, expected to look like:
	//	 x5 x6 x4
	//	   x3 x2 : x4
	//	     x1 : x2
	//	   x7 : x6
	BayesTree<GaussianConditional> chordalBayesNet = *GaussianMultifrontalSolver(smoother).eliminate();
	GaussianISAM bayesTree(chordalBayesNet);

	// Conditional density elements reused by both tests
	const Vector sigma = ones(2);
	const Matrix I = eye(2), A = -0.00429185*I;

	// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
	GaussianBayesNet expected1;
	// Why does the sign get flipped on the prior?
	GaussianConditional::shared_ptr
		parent1(new GaussianConditional(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2)));
	expected1.push_front(parent1);
	push_front(expected1,ordering[X(1)], zero(2), I/sigmax7, ordering[X(7)], A/sigmax7, sigma);
	GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)]);
	EXPECT(assert_equal(expected1,actual1,tol));

//	// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
//	GaussianBayesNet expected2;
//	GaussianConditional::shared_ptr
//			parent2(new GaussianConditional(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2)));
//		expected2.push_front(parent2);
//	push_front(expected2,ordering[X(7)], zero(2), I/sigmax1, ordering[X(1)], A/sigmax1, sigma);
//	GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]);
//	EXPECT(assert_equal(expected2,actual2,tol));

	// Check the joint density P(x1,x4), i.e. with a root variable
	GaussianBayesNet expected3;
	GaussianConditional::shared_ptr
			parent3(new GaussianConditional(ordering[X(4)], zero(2), I/sigmax4, ones(2)));
		expected3.push_front(parent3);
	double sig14 = 0.784465;
	Matrix A14 = -0.0769231*I;
	push_front(expected3,ordering[X(1)], zero(2), I/sig14, ordering[X(4)], A14/sig14, sigma);
	GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)]);
	EXPECT(assert_equal(expected3,actual3,tol));

//	// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
//	GaussianBayesNet expected4;
//	GaussianConditional::shared_ptr
//			parent4(new GaussianConditional(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2)));
//		expected4.push_front(parent4);
//	double sig41 = 0.668096;
//	Matrix A41 = -0.055794*I;
//	push_front(expected4,ordering[X(4)], zero(2), I/sig41, ordering[X(1)], A41/sig41, sigma);
//	GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]);
//	EXPECT(assert_equal(expected4,actual4,tol));
}

/* ************************************************************************* */
TEST_UNSAFE(BayesTree, simpleMarginal)
{
  GaussianFactorGraph gfg;

  Matrix A12 = Rot2::fromDegrees(45.0).matrix();

  gfg.add(0, eye(2), zero(2), noiseModel::Isotropic::Sigma(2, 1.0));
  gfg.add(0, -eye(2), 1, eye(2), ones(2), noiseModel::Isotropic::Sigma(2, 1.0));
  gfg.add(1, -eye(2), 2, A12, ones(2), noiseModel::Isotropic::Sigma(2, 1.0));

  Matrix expected(GaussianSequentialSolver(gfg).marginalCovariance(2));
  Matrix actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2));

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <stdexcept>
#include <cmath>

TEST(MathFunctions, expm1) {
  using stan::math::expm1;
  using std::exp;
  EXPECT_FLOAT_EQ(exp(-14.2) - 1, expm1(-14.2));
  EXPECT_FLOAT_EQ(exp(0) - 1, expm1(0));
  EXPECT_FLOAT_EQ(exp(172.987) - 1, expm1(172.987));
  EXPECT_FLOAT_EQ(-1, expm1(-std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, expm1_inf_return) {
  EXPECT_EQ(std::numeric_limits<double>::infinity(),
            stan::math::expm1(std::numeric_limits<double>::infinity()));
}

TEST(MathFunctions, expm1_nan) {
  using stan::math::expm1;
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::expm1(std::numeric_limits<double>::quiet_NaN()));
}