#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
   Matrix3f A;
   A << 1, 2, 5,
        2, 1, 4,
        3, 0, 3;
   cout << "Here is the matrix A:\n" << A << endl;
   FullPivLU<Matrix3f> lu_decomp(A);
   // lu_decomp.setThreshold(1e-5);
   cout << "The rank of A is " << lu_decomp.rank() << endl;
   cout << "Here is a matrix whose columns form a basis of the null-space of A:\n"
        << lu_decomp.kernel() << endl;
   cout << "Here is a matrix whose columns form a basis of the column-space of A:\n"
        << lu_decomp.image(A) << endl; // yes, have to pass the original A
}


//####### Test module for special functions ####################################

//Define Module name
 #define BOOST_TEST_MODULE "math/special_functions"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <picsar_qed/math/special_functions.hpp>

#include <array>

using namespace picsar::multi_physics::math;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-12;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-4;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

// ------------- Tests --------------

// ***Test Bessel functions

template<typename T, typename WHATEVER>
constexpr void bessel_functions_test(WHATEVER t_v, WHATEVER t_x, WHATEVER t_exp)
{
    const T v = static_cast<T>(t_v);
    const T x = static_cast<T>(t_x);
    const T exp = static_cast<T>(t_exp);

    T res = k_v(v,x);
    BOOST_CHECK_SMALL((res-exp)/exp, tolerance<T>());
}

template<typename T>
constexpr void test_case()
{
    bessel_functions_test<T>(1.0/3.0, 0.5, 0.989031074246724);
    bessel_functions_test<T>(1.0/3.0, 1.0, 0.438430633441534);
    bessel_functions_test<T>(1.0/3.0, 2.0, 0.116544961296165);
    bessel_functions_test<T>(2.0/3.0, 0.5, 1.205930464720336);
    bessel_functions_test<T>(2.0/3.0, 1.0, 0.494475062104208);
    bessel_functions_test<T>(2.0/3.0, 2.0, 0.124838927488128);
}

BOOST_AUTO_TEST_CASE( picsar_bessel_functions )
{
    test_case<double>();
    test_case<float>();
}

// *******************************


#define BOOST_TEST_MODULE test_utils

#include <boost/test/unit_test.hpp>
#include <Utils/utils.h>

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_SUITE(utils_boost)

    BOOST_AUTO_TEST_CASE(annual_cap1) {
        BOOST_TEST_MESSAGE("Testing annual_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 121; // (100*(1.1)^2)

        auto calculated_value = annual_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_discount1) {
        BOOST_TEST_MESSAGE("Testing annual_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 121;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 100; // (121/(1.1)^2)

        auto calculated_value = discount_annually(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_cap1) {
        BOOST_TEST_MESSAGE("Testing period_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 110.25; // (100*(1.05)^2)

        auto calculated_value = period_capitalization(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_discount1) {
        BOOST_TEST_MESSAGE("Testing period_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 110.25;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 100; // (110.25/(1.05)^2)

        auto calculated_value = discount_by_periods(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(continuous_cap1) {
        BOOST_TEST_MESSAGE("Testing continuous_cap1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 122.140275816; // 100 * e^(0.10*2) rounded to second

        auto calculated_value = continuous_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(continuous_discount1) {
        BOOST_TEST_MESSAGE("Testing continuous_discount1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 122.140275816;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 100; // 122.140275816 / e^(0.10*2) rounded to second

        auto calculated_value = discount_continuously(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known discounted_value: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(fwd_rate1) {
        BOOST_TEST_MESSAGE("Testing fwd_rate1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double zero_coupon_total = 0.04;
        int years_total = 2;

        double zero_coupon_partial = 0.03;
        int years_partial = 1;

        double theoretical_value = 0.05; // (100*(1.1)^2) rounded to second

        auto calculated_value = forward_rate(zero_coupon_total, years_total, zero_coupon_partial, years_partial);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_to_cont1) {
        BOOST_TEST_MESSAGE("Testing annual_to_cont1");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double annual_rate = 0.12;

        double theoretical_value = 0.1133286853; // ln(1.12)

        auto calculated_value = annual_to_continuous_rate(1, annual_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-10));
    }

    BOOST_AUTO_TEST_CASE(cont_to_annual) {
        BOOST_TEST_MESSAGE("Testing cont_to_annual");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double continuous_rate = 0.11332868531; //(e^0.1)-1

        double theoretical_value = 0.12; // (e^0.1)-1

        auto calculated_value = continuous_to_annual_rate(1, continuous_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-9));
    }

    BOOST_AUTO_TEST_CASE(total_day_count, *utf::tolerance(0.0001)) {
        BOOST_TEST_MESSAGE("Testing cont_to_annual");
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double myDoubles[] = {185.0 / 360, 182.0 / 360, 182.0 / 360, 182.0 / 360};
        std::vector<double> dayCountFractionVector(myDoubles, myDoubles + sizeof(myDoubles) / sizeof(double));

        std::vector<double> calculated_values = getTotalDayCountFractionVector(dayCountFractionVector);

        double myResults[] = {0.513888888889, 1.019444444444, 1.525000000000, 2.030555555556};
        std::vector<double> expected_values(myResults, myResults + sizeof(myResults) / sizeof(double));

        BOOST_TEST(calculated_values == expected_values, boost::test_tools::per_element());

    }

BOOST_AUTO_TEST_SUITE_END()


/**
 * \file boost/numeric/ublasx/operation/rank.hpp
 *
 * \brief Rank of a matrix.
 *
 * The rank of a matrix is the number of linearly independent rows or columns.
 *
 * The \c rank function provides an estimate of the number of linearly
 * independent rows or columns of a matrix.
 * There are a number of ways to compute the rank of a matrix.
 * The currently adopted method is  based on the singular value decomposition
 * (SVD) which is the most time consuming, but also the most reliable.
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_RANK_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_RANK_HPP

#include <algorithm>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/eps.hpp>
#include <boost/numeric/ublasx/operation/max.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/operation/svd.hpp>
#include <boost/numeric/ublasx/operation/which.hpp>

namespace boost { namespace numeric { namespace ublasx {

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

/**
 * \brief Estimate the rank as the number of singular values of \a A that are
 *  greater than a given tolerance.
 * \tparam MatrixExprT The type of the input matrix expression.
 * \tparam RealT The floating-point type of the tolerance.
 * \param A The input matrix expression.
 * \param tol The tolerance.
 * \return The number of singular values of \a A that are greater than \a tol.
 */
template <typename MatrixExprT, typename RealT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type rank(matrix_expression<MatrixExprT> const& A, RealT tol)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;

	vector<real_type> s = svd_values(A);
	return size(which(s, ::std::bind2nd(::std::greater<real_type>(), tol)));
}

/**
 * \brief Estimate the rank as the number of singular values of \a A that are
 *  greater than the default tolerance.
 * \tparam MatrixExprT The type of the input matrix expression.
 * \param A The input matrix expression.
 * \return The number of singular values of \a A that are greater than \a tol.
 *
 * The default tolerance is
 * \f[
 * 	 \max(n,m) \|A\|_2 {\epsilon}_m
 * \f]
 * where \f$n\f$ is the number of rows of \f$A\f$, \f$m\f$ is the number of
 * columns of \f$A\f$, and \f${\epsilon}_m\f$ is the floating-point machine
 * precision.
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type rank(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;

	vector<real_type> s = svd_values(A);
	real_type tol = ::std::max(num_rows(A), num_columns(A))*eps(max(s)); // note: max(s) == norm_2(A)
	return size(which(s, ::std::bind2nd(::std::greater<real_type>(), tol)));
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_RANK_HPP


/*
This program is free software; you can redistribute it and/or modify it under
the terms of the European Union Public Licence - EUPL v.1.1 as published by
the European Commission.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
for more details.

You should have received a copy of the European Union Public Licence - EUPL v.1.1
along with this program.

Further information about the European Union Public Licence - EUPL v.1.1 can
also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
STA uses interpolators based on the State Vector structures defined in "Astro-Core/statevector.h"
file. Vector and matrices operations are performed using the Eigen library.

Given a set of data points (x1 , y1 ) . . . (xn , yn ), the STA interpolators compute a
continuous interpolating function y(x) such that y(xi ) = yi

The interpolation is piecewise smooth, and its behavior at the end-points is determined by the
type of interpolation used.

*/

/*
 ------------------ Author: Guillermo Ortega ESA  -------------------------------------------
 */

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

#include <Astro-Core/statevector.h>

#include "Interpolators.h"

using namespace sta;
using namespace Eigen;

/*
 Linear interpolation between two state vectors x0 and x1.
interval is the length of time (in seconds) between the two state vectors.
t is a value between 0 and 1 that specifies the time within the interval.

If the two known points are given by the coordinates (x_0,y_0)
and (x_1,y_1), the linear interpolant is the straight line
between these points. For a value x in the interval (x_0, x_1),
the value y along the straight line is given from the equation

    \frac{y - y_0}{x - x_0} = \frac{y_1 - y_0}{x_1 - x_0}

Solving this equation for y, which is the unknown value at x, gives

    y = y_0 + (x-x_0)\frac{y_1 - y_0}{x_1-x_0}

which is the formula for linear interpolation in the interval
(x_0,x_1). Outside this interval, the formula is identical
to linear extrapolation.

*/
sta::StateVector linearInterpolate(const sta::StateVector& x0,    // First state vector
                                         const sta::StateVector& x1,     // Second state vector
                                         double t,                       // Time within x0 an x1
                                         double interval)                // Time distance between x1 and x1
{
    sta::StateVector result;
    result.position = x0.position + t * ((x1.position - x0.position) * (1.0 / interval));
    result.velocity = x0.velocity + t * ((x1.velocity - x0.velocity) * (1.0 / interval));
    return result;
}

// Routine programmed C. Laurel
// Cubic interpolation between two state vectors v0 and v1.
// Interval is the length of time (in seconds) between the two state vectors.
// t is a value between 0 and 1 that specifies the time within the interval.
sta::StateVector cubicInterpolate(const sta::StateVector& v0,
                                         const sta::StateVector& v1,
                                         double t,
                                         double interval)
{
    double t2 = t * t;
    double t3 = t2 * t;
    Vector3d a = 2.0 * (v0.position - v1.position) + interval * (v1.velocity + v0.velocity);
    Vector3d b = 3.0 * (v1.position - v0.position) - interval * (2.0 * v0.velocity + v1.velocity);
    Vector3d c = v0.velocity * interval;
    Vector3d d = v0.position;

    sta::StateVector result;
    result.position = a * t3 + b * t2 + c * t + d;
    result.velocity = (a * (3.0 * t2) + b * (2.0 * t) + c) * (1.0 / interval);

    return result;
}


#include <iostream>
#include <boost/math/constants/constants.hpp> // pi
#include <static_math/complex.h> // constexpr complex class
#include "static_poly_io.hpp"

using std::cout;

/*** Greatest common divisor ***/
constexpr int gcd(int m, int n) {
   while (n != 0) {
      m %= n;
      if (m == 0) return n;
      n %= m;
    }
    return m;
}

/*** The Nth cyclotomic polynomial, computed with complex numbers ***/
template <int N>
constexpr static_poly<smath::complex<double>, N+1> cyclotomic() {
    static_assert(N > 0, "No nonpositive numbers please!");
    using namespace boost::math::double_constants;

    static_poly<smath::complex<double>, N+1> cyc{1};

    for (int k = 1; k <= N; ++k) {
        if (gcd(k, N) == 1) {
            static_poly<smath::complex<double>, 2> x{-smath::polar(1., 2.*k*pi / N), 1};
            cyc = detail::mul(cyc, x);
        // alas, std::polar(1., 2.*k*pi / N) is not constexpr
        // furthermore, std::cos/std::sin are not constexpr (why!?)
        }
    }
    return cyc;
}

/*** Euler's totient function... ***/
template <int N>
constexpr int euler_totient() {
    return cyclotomic<N>().degree();
}

template <int N>
struct foo {
    constexpr int val() const {
        return N;
    }
};

int main() {
    constexpr auto phi1 = cyclotomic<1>();
    constexpr auto phi2 = cyclotomic<2>();
    constexpr auto phi3 = cyclotomic<3>();
    constexpr auto phi4 = cyclotomic<4>();
    constexpr auto phi5 = cyclotomic<5>();
    constexpr auto phi6 = cyclotomic<6>();
    constexpr auto phi7 = cyclotomic<7>();
    constexpr auto phi8 = cyclotomic<8>();
    constexpr auto phi9 = cyclotomic<9>();
    constexpr auto phi35 = cyclotomic<35>();

    cout << phi1 << '\n'
         << phi2 << '\n'
         << phi3 << '\n'
         << phi4 << '\n'
         << phi5 << '\n'
         << phi6 << '\n'
         << phi7 << '\n'
         << phi8 << '\n'
         << phi9 << "\n\n"
         << phi1*phi2 << '\n'
         << phi1*phi3 << '\n'
         << phi1*phi2*phi4 << '\n'
         << phi1*phi5 << '\n'
         << phi1*phi2*phi3*phi6 << '\n'
         << phi1*phi7 << '\n'
         << phi1*phi2*phi4*phi8 << '\n'
         << phi1*phi3*phi9 << "\n\n"
         << phi35 << '\n';

    /* Use euler's totient function as a template parameter! */
    foo<euler_totient<76>()> myfoo;
    cout << myfoo.val() << '\n';

    return 0;
}


//
// Created by Lu WJ on 5/7/2017 AD.
//
#include "SMP/Matrix.hpp"
#include "SMP/literal.hpp"

#include <NTL/ZZ.h>
#include <iostream>
#include <sstream>
#include <cassert>
#include <fstream>

void zeros(Matrix *mat, long N) {
    if (!mat) return;
    mat->SetDims(N, N);
    for (long i = 0; i < N; i++) {
        for (long j = 0; j < N; j++)
            (*mat)[i][j] = 0L;
    }
}

void add(PlainVec *a, const PlainVec &b) {
    if (!a or a->length() != b.length())
        return;
    for (long i = 0; i < b.length(); i++)
        a->put(i, a->get(i) + b[i]);
}

void add(Matrix *a, const Matrix &b) {
    if (!a) return;
    assert(a->NumRows() == b.NumRows());
    assert(a->NumCols() == b.NumCols());
    for (long r = 0; r < b.NumRows(); r++) {
        for (long c = 0; c < b.NumCols(); c++) {
            (*a)[r][c] += b[r][c];
        }
    }
}

void transpose(Matrix *T, const Matrix &mat) {
    if (!T) return;
    T->kill();
    T->SetDims(mat.NumCols(), mat.NumRows());
    for (long r = 0; r < mat.NumRows(); r++) {
        for (long c = 0; c < mat.NumCols(); c++)
            (*T)[c][r] = mat[r][c];
    }
}

void randomize(Matrix *mat) {
    if (!mat)
        return;
    for (long r = 0; r < mat->NumRows(); r++) {
        for (long c = 0; c < mat->NumCols(); c++) {
            (*mat)[r][c] = NTL::RandomBnd(20) - 10;
        }
    }
}

bool is_same(const Matrix &a, const Matrix &b) {
    if (a.NumRows() != b.NumRows() || a.NumCols() != b.NumCols())
        return false;
    for (long r = 0; r < a.NumRows(); r++) {
        for (long c = 0; c < a.NumCols(); c++) {
            if (a[r][c] != b[r][c]) {
                std::cerr << a[r][c] << "!=" << b[r][c] << "(" << r << "," << c << ")" << std::endl;
                return false;
            }
        }
    }
    return true;
}

bool is_same(const Matrix &a, const Matrix &b, long modulus) {
    if (a.NumRows() != b.NumRows() || a.NumCols() != b.NumCols())
        return false;
    for (long r = 0; r < a.NumRows(); r++) {
        for (long c = 0; c < a.NumCols(); c++) {
            long diff = (a[r][c] - b[r][c]) % modulus;
            if (diff != 0) {
                std::cerr << a[r][c] << "!=" << b[r][c] << "(" << r << "," << c << ")" << std::endl;
                return false;
            }
        }
    }
    return true;
}

PlainVec mul(const Matrix &m, const PlainVec &v) {
    assert(m.NumCols() == v.length());
    PlainVec result;
    result.SetLength(m.NumRows());
    for (long r = 0; r < m.NumRows(); r++) {
        val_t sum{0};
        for (long c = 0; c < m.NumCols(); c++) {
            sum += m[r][c] * v[c];
        }
        result.put(r, sum);
    }
    return result;
}

Matrix mul(const Matrix &a, const Matrix &b) {
    assert(a.NumCols() == b.NumRows());
    Matrix result;
    result.SetDims(a.NumRows(), b.NumCols());
    for (long r = 0; r < a.NumRows(); r++) {
        for (long c = 0; c < b.NumCols(); c++) {
            val_t sum{0};
            for (long k = 0; k < b.NumRows(); k++)
                sum += a[r][k] * b[k][c];
            result[r][c] = sum;
        }
    }
    return result;
}

bool load_matrix(std::istream &in, Matrix *mat) {
    if (in.bad() || in.eof() || !mat)
        return false;

    std::string line;
    int line_num = 0;
    for (long r = 0; !in.eof() && r < mat->NumRows(); r++) {
        std::getline(in, line);
        auto fields = splitBySpace(line);
        if (fields.size() != mat->NumCols()) {
            std::cerr << "Warn: need " << mat->NumCols() << " columns, but got " << fields.size() <<
                " values in the " << line_num + 1<< " line." << std::endl;
            return false;
        }
        size_t pos;
        for (size_t c = 0; c < fields.size(); c++) {
            long val = std::stol(fields[c], &pos, 10);
            if (pos != fields[c].size()) {
                std::cerr << "Warn: invalid value " << fields[c] << " in line " << line_num + 1 << std::endl;
                val = 0;
            }
            (*mat)[line_num][c] = val;
        }
        line_num += 1;
    }

    if (line_num != mat->NumRows()) {
        std::cerr << "Warn: need " << mat->NumRows() << " rows but got " << line_num << " rows." << std::endl;
        return false;
    }
    return true;
}

bool load_matrix(Matrix *mat, const std::string &file) {
    if (!mat) {
        std::cerr << "Error: can not load matrix in an empty pointer." << std::endl;
        return false;
    }

    std::ifstream in(file);
    if (!in.is_open()) {
        std::cerr << "Error: can not open file " << file << std::endl;
        return false;
    }

    std::string header_line;
    std::getline(in, header_line);
    std::stringstream header(header_line);
    char sharp;
    long rows, cols;
    header >> sharp >> rows >> cols;
    if (sharp != '#' || rows <= 0 || cols <= 0) {
        std::cerr << "Error: invalid header " << file << std::endl;
        return false;
    }
    mat->SetDims(rows, cols);
    bool ok = load_matrix(in, mat);
    in.close();
    return ok;
}

bool save_matrix(std::ostream &out, const Matrix &mat) {
    if (out.bad() || out.eof())
        return false;
    if (mat.NumRows() <= 0 || mat.NumCols() <= 0)
        return false;
    for (long r = 0; r < mat.NumRows(); r++) {
        for (long c = 0; c + 1 < mat.NumCols(); c++) {
            out << mat[r][c] << " ";
        }
        out << mat[r][mat.NumCols() - 1] << std::endl;
    }
    return out.good();
}

bool save_matrix(const Matrix &mat, const std::string &file) {
    std::ofstream out(file);
    if (!out.is_open()) {
        std::cerr << "Error: can not open file " << file << std::endl;
        return false;
    }
    out << "#" << mat.NumRows() << " " << mat.NumCols() << std::endl;
    bool ok = save_matrix(out, mat);
    out.close();
    return ok;
}

bool load_vector(std::istream &in, PlainVec *vec) {
    if (in.bad() || in.eof() || !vec)
        return false;
    std::string line;
    std::getline(in, line);
    const std::vector<std::string> fields = splitBySpace(line);
    if (fields.size() != vec->length()) {
        std::cerr << "Error: need " << vec->length()
                  << " elements but get " << fields.size() << "." << std::endl;
        return false;
    }
    size_t pos;
    for (size_t c = 0; c < fields.size(); c++) {
        long val = std::stol(fields.at(c), &pos, 10);
        if (pos != fields[c].size()) {
            std::cerr << "Warn: invalid value: " << fields[c] << std::endl;
            val = 0L;
        }
        vec->put(c, val);
    }
    return true;
}

bool load_vector(PlainVec *vec, const std::string &file) {
    std::ifstream in(file);
    if (!in.is_open()) {
        std::cerr << "Error: can not open file " << file << std::endl;
        return false;
    }
    std::string header;
    std::getline(in, header);
    std::stringstream sstream(header);
    char sharp = '\0';
    long length = 0;
    sstream >> sharp >> length;
    if (sharp != '#' || length <= 0) {
        std::cerr << "Error: invalid header of " << file << std::endl;
        return false;
    }

    vec->SetLength(length);
    bool ok = load_vector(in, vec);
    in.close();
    return ok;
}

bool save_vector(const PlainVec &vec, const std::string &file) {
    std::ofstream out(file);
    if (!out.is_open()) {
        std::cerr << "Error: can not open file " << file << std::endl;
        return false;
    }

    out << "#" << vec.length() << std::endl;
    bool ok = save_vector(out, vec);
    out.close();
    return ok;
}

bool save_vector(std::ostream &out, const PlainVec &vec) {
    if (out.bad())
        return false;
    if (vec.length() == 0)
        return true;
    for (long c = 0; c + 1< vec.length(); c++)
        out << vec[c] << " ";
    out << vec[vec.length() - 1] << std::endl;
    return out.good();
}


#include "RSA.h"
#include "math/Euclidean.h"
#include "Encryptor.h"
#include "Decryptor.h"
#include <exception>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;
using namespace Crypto;

RSA::RSA(int256_t p, int256_t q, int256_t r) : p(0), q(0), m(0), phi_of_m(0), r(0), public_key(nullptr), private_key(nullptr)
{
  try
  {
    setParameters(p, q, r);
    calcKeys();
  }
  catch (std::exception &e)
  {
    //std::cout << e.what();
    throw; // Rethrow exception.
  }
}

void RSA::setParameters(int256_t p, int256_t q, int256_t r)
{
  this->p = p;
  this->q = q;
  this->r = r;
  this->m = p * q;
  this->phi_of_m = calcPhi(p, q);

  if (!isPrime(p) || !isPrime(q)) // p and q must be prime numbers
  {
    throw std::exception("[ERROR] p or q is not a prime number!");
  }
  else if ((r < m) && (r > 1) && (Euclidean::euclidean(r, phi_of_m) != 1)) // r and phi of m must be coprime
  {
    throw std::exception("[ERROR] r is not equal or less than p * q (=> m), or r and phi of m are not coprime!");
  }
}

void RSA::calcKeys()
{
  if (private_key != nullptr || public_key != nullptr)
  {
    throw std::exception("[ERROR] Keys already calculated!");
  }

  calcPublicKey();
  calcPrivateKey();
}

void RSA::calcPrivateKey()
{

  int256_t a = this->phi_of_m;
  int256_t b = this->r;

  int256_t s = 0;
  int256_t x = 0;

  //calculates the secret key
  Euclidean::extendedEuclidean(a, b, &x, &s);

  s = s < 0 ? makePositive(s, this->phi_of_m) : s;

  this->private_key = new PrivateKey{ s, this->p, this->q };
}

void RSA::calcPublicKey()
{
  this->public_key = new PublicKey{ this->r, this->m };
}

const PublicKey* RSA::getPublicKey() const
{
  return public_key;
}

const PrivateKey* RSA::getPrivateKey() const
{
  return private_key;
}

bool RSA::isPrime(int256_t numb)
{
  int it;
  for (it = 2; it < numb; it++)
  {
    if ((numb % it) == 0)
      return false;
  }

  return true;
}

int256_t RSA::calcPhi(int256_t a, int256_t b)
{
  if (!isPrime(a) || !isPrime(b))
  {
    return 0;
  }

  return (a - 1) * (b - 1);
}

int256_t RSA::makePositive(int256_t numb, int256_t mod) const
{
  int256_t tmp = numb;
  while (tmp < 0)
  {
    tmp += mod;
  }

  return tmp;
}

CryptoString RSA::encrypt(string str)
{
  Encryptor enc(public_key);
  CryptoString out = enc.encryptString(str);
  return out;
}

string RSA::decrypt(CryptoString str)
{
  Crypto::Decryptor dec(private_key);
  string res = dec.decryptString(str);
  return res;
}

CryptoChar RSA::encrypt(char ch)
{
  Encryptor enc(public_key);
  CryptoChar out = enc.encryptChar(ch);
  return out;
}

char RSA::decrypt(CryptoChar ch)
{
  Crypto::Decryptor dec(private_key);
  char res = dec.decryptChar(ch);
  return res;
}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearfeelementmatrix.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 4, 4> MyLinearFEElementMatrix::Eval(
    const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());
  // Matrix for returning element matrix
  Eigen::Matrix<double, 4, 4> elem_mat;

  //====================
  // Your code goes here

  // STEP 1: Case distinction depending on type of the mesh element
  if (ref_el.ToString() == "TRIA"){
      // bottom row and rightmost col are not used
      elem_mat << 2., 1., 1., 0.,
                  1., 2., 1., 0.,
                  1., 1., 2., 0.,
                  0., 0., 0., 0.;
      // multiply with size |K|
      elem_mat /= 12.;
    }
    else{
     //"QUAD":
      elem_mat << 4., 2., 1., 2.,
                  2., 4., 2., 1.,
                  1., 2., 4., 2.,
                  2., 1., 2., 4.;
      // multiply with size |K|
      elem_mat /= 36.;
  }

  // multiply with the size of the mesh element
  double K = lf::geometry::Volume(*geo_ptr);
  elem_mat *= K;

  // STEP 2: linearly add the computed -Delta u term
  lf::uscalfe::LinearFELaplaceElementMatrix emp;

  elem_mat += emp.Eval(cell);

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

  return elem_mat;
}
/* SAM_LISTING_END_1 */
}  // namespace ElementMatrixComputation


//
// Copyright (c) 2018-2019 CNRS INRIA
//

#include "pinocchio/autodiff/cppad.hpp"
#include <cppad/speed/det_by_minor.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

  BOOST_AUTO_TEST_CASE(test_example1_cppad)
  {
    using CppAD::AD;
    using CppAD::NearEqual;
    using Eigen::Matrix;
    using Eigen::Dynamic;
    //
    typedef Matrix< AD<double> , Dynamic, 1 > eigen_vector;
    //
    // some temporary indices
    size_t i, j;

    // domain and range space vectors
    size_t n  = 10, m = n;
    eigen_vector a_x(n), a_y(m);

    // set and declare independent variables and start tape recording
    for(j = 0; j < n; j++)
    {
      a_x[(Eigen::DenseIndex)j] = double(1 + j);
    }
    CppAD::Independent(a_x);

    // evaluate a component wise function
    a_y = a_x.array() + a_x.array().sin();

    // create f: x -> y and stop tape recording
    CppAD::ADFun<double> f(a_x, a_y);

    // compute the derivative of y w.r.t x using CppAD
    CPPAD_TESTVECTOR(double) x(n);
    for(j = 0; j < n; j++)
    {
      x[j] = double(j) + 1.0 / double(j+1);
    }
    CPPAD_TESTVECTOR(double) jac = f.Jacobian(x);

      // check Jacobian
    double eps = 100. * CppAD::numeric_limits<double>::epsilon();
    for(i = 0; i < m; i++)
    {
      for(j = 0; j < n; j++)
      {
        double check = 1.0 + cos(x[i]);
        if( i != j ) check = 0.0;
          BOOST_CHECK(NearEqual(jac[i * n + j], check, eps, eps));
      }
    }
  }

  BOOST_AUTO_TEST_CASE(test_example2_cppad)
  {
    using CppAD::AD;
    using CppAD::NearEqual;
    using Eigen::Matrix;
    using Eigen::Dynamic;
    //
    typedef Matrix< double     , Dynamic, Dynamic > eigen_matrix;
    typedef Matrix< AD<double> , Dynamic, Dynamic > eigen_ad_matrix;
    //
    typedef Matrix< double ,     Dynamic , 1>       eigen_vector;
    typedef Matrix< AD<double> , Dynamic , 1>       eigen_ad_vector;
    // some temporary indices
    size_t i, j;

    // domain and range space vectors
    size_t size = 3, n  = size * size, m = 1;
    eigen_ad_vector a_x(n), a_y(m);
    eigen_vector x(n);

    // set and declare independent variables and start tape recording
    for(i = 0; i < size; i++)
    {
      for(j = 0; j < size; j++)
      {     // lower triangular matrix
        a_x[(Eigen::DenseIndex)(i * size + j)] = x[(Eigen::DenseIndex)(i * size + j)] = 0.0;
        if( j <= i )
          a_x[(Eigen::DenseIndex)(i * size + j)] = x[(Eigen::DenseIndex)(i * size + j)] = double(1 + i + j);
      }
    }
    CppAD::Independent(a_x);

    // copy independent variable vector to a matrix
    eigen_ad_matrix a_X(size, size);
    eigen_matrix X(size, size);
    for(i = 0; i < size; i++)
    {
      for(j = 0; j < size; j++)
      {
        X((Eigen::DenseIndex)i, (Eigen::DenseIndex)j)   = x[(Eigen::DenseIndex)(i * size + j)];
        // If we used a_X(i, j) = X(i, j), a_X would not depend on a_x.
        a_X((Eigen::DenseIndex)i, (Eigen::DenseIndex)j) = a_x[(Eigen::DenseIndex)(i * size + j)];
      }
    }

    // Compute the log of determinant of X
    a_y[0] = log( a_X.determinant() );

    // create f: x -> y and stop tape recording
    CppAD::ADFun<double> f(a_x, a_y);

    // check function value
    double eps = 100. * CppAD::numeric_limits<double>::epsilon();
    CppAD::det_by_minor<double> det(size);
    BOOST_CHECK(NearEqual(Value(a_y[0]) , log(det(x)), eps, eps));

    // compute the derivative of y w.r.t x using CppAD
    eigen_vector jac = f.Jacobian(x);

    // check the derivative using the formula
    // d/dX log(det(X)) = transpose( inv(X) )
    eigen_matrix inv_X = X.inverse();
    for(i = 0; i < size; i++)
    {
      for(j = 0; j < size; j++)
        BOOST_CHECK(NearEqual(jac[(Eigen::DenseIndex)(i * size + j)],
                              inv_X((Eigen::DenseIndex)j, (Eigen::DenseIndex)i),
                              eps,
                              eps));
    }
  }

  BOOST_AUTO_TEST_CASE(test_sincos)
  {
    using CppAD::AD;
    using CppAD::NearEqual;
    double eps99 = 99.0 * std::numeric_limits<double>::epsilon();

    typedef AD<double> AD_double;

    double x0 = 1.;
    CPPAD_TESTVECTOR(AD_double) x(1), y(1), z(1);
    x[0] = x0;
    CppAD::Independent(x);

    y[0] = CppAD::cos(x[0]);
    BOOST_CHECK(NearEqual(y[0],std::cos(x0),eps99,eps99));
    CppAD::ADFun<double> fcos(x, y);

    CPPAD_TESTVECTOR(double) x_eval(1);
    x_eval[0] = x0;
    CPPAD_TESTVECTOR(double) dy(1);
    dy = fcos.Jacobian(x_eval);
    BOOST_CHECK(NearEqual(dy[0],-std::sin(x0),eps99,eps99));

    CppAD::Independent(x);
    z[0] = CppAD::sin(x[0]);
    BOOST_CHECK(NearEqual(z[0],std::sin(x0),eps99,eps99));

    CppAD::ADFun<double> fsin(x, z);

    CPPAD_TESTVECTOR(double) dz(1);
    dz = fsin.Jacobian(x_eval);
    BOOST_CHECK(NearEqual(dz[0],std::cos(x0),eps99,eps99));
  }

  BOOST_AUTO_TEST_CASE(test_eigen_support)
  {
    using namespace CppAD;

    // use a special object for source code generation
    typedef AD<double> ADScalar;

    typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;

    ADVector vec_zero(ADVector::Zero(100));
    BOOST_CHECK(vec_zero.isZero());

    ADVector vec_ones(100);
    vec_ones.fill(1);
    BOOST_CHECK(vec_ones.isOnes());

  }

  BOOST_AUTO_TEST_CASE(test_abs)
  {
    CppAD::AD<double> ad_value;
    ad_value = -1.;
    abs(ad_value);
  }

BOOST_AUTO_TEST_CASE(test_atan2)
{
  CppAD::AD<double> theta,x,y;
  x = pinocchio::math::cos(theta); y = pinocchio::math::sin(theta);

  pinocchio::math::atan2(y,x);

}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file register_svd.hpp
 * \brief Header for registerSVD function
 * \details Calculates rigid transformation given two sets of points. Used in
 * stereo_transform_model
 *
 * \author Kirk MacTavish, Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

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

namespace vtr {
namespace vision {

// Returns tf s.t. a = tf * b
template <unsigned int N>
bool registerSVD(const Eigen::Matrix<double, N, Eigen::Dynamic>& a,
                 const Eigen::Matrix<double, N, Eigen::Dynamic>& b,
                 Eigen::Matrix<double, N + 1, N + 1>* tf,
                 const Eigen::Array<double, 1, Eigen::Dynamic>& weights =
                     Eigen::Array<double, 1, Eigen::Dynamic>(1, 0),
                 bool scaling = false) {
  // Types
  // typedef Eigen::Matrix<double,N+1,N+1> Transform;
  typedef Eigen::Matrix<double, N, 1> Point;
  typedef Eigen::Matrix<double, N, Eigen::Dynamic> Points;
  typedef Eigen::Matrix<double, N, N> Square;

  // Check output allocation
  if (!tf) return false;

  // Are we weighting points?
  bool use_weights = weights.cols() > 0;

  // Check input (1 pt for 1D, 2 pts for 2D, 3 pts for 3D)
  if (a.cols() < N || a.cols() != b.cols() ||
      (use_weights && a.cols() != weights.cols()))
    return false;

  // Switch based on whether we're weighting the points
  // See https://igl.ethz.ch/projects/ARAP/svd_rot.pdf
  Point ca, cb;
  Points a0, b0;
  Square cov;
  if (use_weights) {
    // Weight points
    Points aw = a.array().rowwise() * weights;
    Points bw = b.array().rowwise() * weights;

    // Compute centroids
    double w_sum = weights.sum();
    ca = aw.rowwise().sum() / w_sum;
    cb = bw.rowwise().sum() / w_sum;

    // Demean
    a0 = a.colwise() - ca;
    b0 = b.colwise() - cb;

    // Covariance
    cov =
        (a0.array().rowwise() * weights).matrix() * b0.transpose();  // a0*W*b0'
  } else {
    // Compute centroids
    ca = a.rowwise().mean();
    cb = b.rowwise().mean();

    // Demean
    a0 = a.colwise() - ca;
    b0 = b.colwise() - cb;

    // Covariance
    cov = a0 * b0.transpose();
  }

  // SVD
  Eigen::JacobiSVD<Square> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Square V = svd.matrixV();
  // Normally V * U', but then we have to use R'.
  Square R = svd.matrixU() * V.transpose();

  // Check for proper right-hand rotation
  double det = R.determinant();
  if (det < 0. || use_weights) {
    // If we use the weights, we have to remove their influence here
    V.col(N - 1) *= use_weights ? det : -1.;
    // Normally V * U', but then we have to use R'.
    R = svd.matrixU() * V.transpose();
  }

  // Apply scaling if necessary
  if (scaling) {
    double scale = b0.norm() / a0.norm();
    R /= scale;
  }

  // Translation
  Point t = ca - R * cb;

  // Build final transform
  tf->row(N).setZero();
  (*tf)(N, N) = 1.;
  tf->template topLeftCorner<N, N>() = R;
  tf->template topRightCorner<N, 1>() = t;

  // Return
  return true;
}

// Explicit instantiation for common params
extern template bool registerSVD<2>(
    const Eigen::Matrix<double, 2, Eigen::Dynamic>& a,
    const Eigen::Matrix<double, 2, Eigen::Dynamic>& b,
    Eigen::Matrix<double, 2 + 1, 2 + 1>* tf,
    const Eigen::Array<double, 1, Eigen::Dynamic>& weights, bool scaling);
extern template bool registerSVD<3>(
    const Eigen::Matrix<double, 3, Eigen::Dynamic>& a,
    const Eigen::Matrix<double, 3, Eigen::Dynamic>& b,
    Eigen::Matrix<double, 3 + 1, 3 + 1>* tf,
    const Eigen::Array<double, 1, Eigen::Dynamic>& weights, bool scaling);

}  // namespace vision
}  // namespace vtr


#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>

#include <Eigen/Eigen>

#include <unsupported/Eigen/NonLinearOptimization>

#include <ros/ros.h>

struct LMFunctor
{
	// 'm' pairs of (x, f(x))
	Eigen::MatrixXf measuredValues;

	// Compute 'm' errors, one for each data point, for the given parameter values in 'x'
	int operator()(const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
	{
		// 'x' has dimensions n x 1
		// It contains the current estimates for the parameters.

		// 'fvec' has dimensions m x 1
		// It will contain the error for each data point.

		float aParam = x(0);
		float bParam = x(1);
		float cParam = x(2);

		for (int i = 0; i < values(); i++) {
			float xValue = measuredValues(i, 0);
			float yValue = measuredValues(i, 1);

			fvec(i) = yValue - (aParam * xValue * xValue + bParam * xValue + cParam);
		}
		return 0;
	}

	// Compute the jacobian of the errors
	int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
	{
		// 'x' has dimensions n x 1
		// It contains the current estimates for the parameters.

		// 'fjac' has dimensions m x n
		// It will contain the jacobian of the errors, calculated numerically in this case.

		float epsilon;
		epsilon = 1e-7f;

		for (int i = 0; i < x.size(); i++) {
			Eigen::VectorXf xPlus(x);
			xPlus(i) += epsilon;
			Eigen::VectorXf xMinus(x);
			xMinus(i) -= epsilon;

			Eigen::VectorXf fvecPlus(values());
			operator()(xPlus, fvecPlus);

			Eigen::VectorXf fvecMinus(values());
			operator()(xMinus, fvecMinus);

			Eigen::VectorXf fvecDiff(values());
			fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);

			fjac.block(0, i, values(), 1) = fvecDiff;
		}

		return 0;
	}

	// Number of data points, i.e. values.
	int m;

	// Returns 'm', the number of values.
	int values() const { return m; }

	// The number of parameters, i.e. inputs.
	int n;

	// Returns 'n', the number of inputs.
	int inputs() const { return n; }

};

int main(int argc, char *argv[])
{
    // ros::init(argc, argv, "LM_node");
    // ros::start();
	// //
	// Goal
	//
	// Given a non-linear equation: f(x) = a(x^2) + b(x) + c
	// and 'm' data points (x1, f(x1)), (x2, f(x2)), ..., (xm, f(xm))
	// our goal is to estimate 'n' parameters (3 in this case: a, b, c)
	// using LM optimization.
	//

	//
	// Read values from file.
	// Each row has two numbers, for example: 5.50 223.70
	// The first number is the input value (5.50) i.e. the value of 'x'.
	// The second number is the observed output value (223.70),
	// i.e. the measured value of 'f(x)'.

	// 'm' is the number of data points.
	int m = 100;

	// Move the data into an Eigen Matrix.
	// The first column has the input values, x. The second column is the f(x) values.
    float a = 0.1;
    float b = 0.2;
    float c = 0.3;
	Eigen::MatrixXf measuredValues(m, 2);
	for (int i = 0; i < m; i++) {
		measuredValues(i, 0) = (float) rand()/RAND_MAX * 10;
		measuredValues(i, 1) = a*measuredValues(i, 0)*measuredValues(i, 0) + b*measuredValues(i, 0) + c + (float) rand()/RAND_MAX/10;
	}

	// 'n' is the number of parameters in the function.
	// f(x) = a(x^2) + b(x) + c has 3 parameters: a, b, c
	int n = 3;

	// 'x' is vector of length 'n' containing the initial values for the parameters.
	// The parameters 'x' are also referred to as the 'inputs' in the context of LM optimization.
	// The LM optimization inputs should not be confused with the x input values.
	Eigen::VectorXf x(n);
	x(0) = 0.0;             // initial value for 'a'
	x(1) = 0.0;             // initial value for 'b'
	x(2) = 0.0;             // initial value for 'c'

	//
	// Run the LM optimization
	// Create a LevenbergMarquardt object and pass it the functor.
	//

	LMFunctor functor;
	functor.measuredValues = measuredValues;
	functor.m = m;
	functor.n = n;

	Eigen::LevenbergMarquardt<LMFunctor, float> lm(functor);
	int status = lm.minimize(x);
	std::cout << "LM optimization status: " << status << std::endl;

	//
	// Results
	// The 'x' vector also contains the results of the optimization.
	//
	std::cout << "Optimization results" << std::endl;
	std::cout << "\ta: " << x(0) << std::endl;
	std::cout << "\tb: " << x(1) << std::endl;
	std::cout << "\tc: " << x(2) << std::endl;

    // ros::shutdown();
	return 0;
}


/*
 * 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 <boost/math/tools/cohen_acceleration.hpp>
#include <boost/math/constants/constants.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::math::tools::cohen_acceleration;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::constants::pi;

template<typename Real>
class G {
public:
    G(){
        k_ = 0;
    }

    Real operator()() {
        k_ += 1;
        return 1/(k_*k_);
    }

private:
    Real k_;
};

template<typename Real>
void test_pisq_div12()
{
    auto g = G<Real>();
    Real x = cohen_acceleration(g);
    CHECK_ULP_CLOSE(pi<Real>()*pi<Real>()/12, x, 3);
}

template<typename Real>
class Divergent {
public:
    Divergent(){
        k_ = 0;
    }

    // See C3 of: https://people.mpim-bonn.mpg.de/zagier/files/exp-math-9/fulltext.pdf
    Real operator()() {
        using std::log;
        k_ += 1;
        return log(k_);
    }

private:
    Real k_;
};

template<typename Real>
void test_divergent()
{
    auto g = Divergent<Real>();
    Real x = -cohen_acceleration(g);
    CHECK_ULP_CLOSE(log(pi<Real>()/2)/2, x, 80);
}

int main()
{
    test_pisq_div12<float>();
    test_pisq_div12<double>();
    test_pisq_div12<long double>();

    test_divergent<float>();
    test_divergent<double>();
    test_divergent<long double>();

    #ifdef BOOST_HAS_FLOAT128
    test_pisq_div12<float128>();
    test_divergent<float128>();
    #endif
    return boost::math::test::report_errors();
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Eigen>
#include <cmath>

namespace fluid {
namespace algorithm {

class SpectralShape
{

  using ArrayXd = Eigen::ArrayXd;

public:
  SpectralShape(index maxFrame) : mMagBuffer(maxFrame) {}

  void processFrame(Eigen::Ref<ArrayXd> in)
  {
    using namespace std;
    double const epsilon = std::numeric_limits<double>::epsilon();

    ArrayXd x = in.max(epsilon);
    index   size = x.size();
    double  xSum = x.sum();
    ArrayXd xSquare = x.square();
    ArrayXd lin = ArrayXd::LinSpaced(size, 0, size - 1);
    double  centroid = (x * lin).sum() / xSum;
    double  spread = (x * (lin - centroid).square()).sum() / xSum;
    double  skewness =
        (x * (lin - centroid).pow(3)).sum() / (spread * sqrt(spread) * xSum);
    double kurtosis =
        (x * (lin - centroid).pow(4)).sum() / (spread * spread * xSum);
    double flatness = exp(x.log().mean()) / x.mean();
    double rolloff = size - 1;
    double cumSum = 0;
    double target = 0.95 * xSquare.sum();
    for (index i = 0; cumSum <= target && i < size; i++)
    {
      cumSum += xSquare(i);
      if (cumSum > target)
      {
        rolloff = i - (cumSum - target) / xSquare(i);
        break;
      }
    }
    double crest = x.maxCoeff() / sqrt(x.square().mean());

    mOutputBuffer(0) = centroid;
    mOutputBuffer(1) = sqrt(spread);
    mOutputBuffer(2) = skewness;
    mOutputBuffer(3) = kurtosis;
    mOutputBuffer(4) = rolloff;
    mOutputBuffer(5) = 20 * log10(max(flatness, epsilon));
    mOutputBuffer(6) = 20 * log10(max(crest, epsilon));
  }

  void processFrame(const RealVector& input, RealVectorView output)
  {
    assert(output.size() == 7); // TODO
    ArrayXd in = _impl::asEigen<Eigen::Array>(input);
    processFrame(in);
    output = _impl::asFluid(mOutputBuffer);
  }

private:
  ArrayXd mMagBuffer;
  ArrayXd mOutputBuffer{7};
};

} // namespace algorithm
} // namespace fluid


/*
 * TPSInterpolate.hpp
 * license: http://www.boost.org/LICENSE_1_0.txt
 *  Created on: 18 Aug 2010
 *      Author: Peter Stroia-Williams
 */

#ifndef TPSINTERPOLATE_HPP_
#define TPSINTERPOLATE_HPP_

#include <vector>
#include <cmath>

#include <boost/array.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

//using namespace boost::numeric::ublas;
//using namespace boost;
//using namespace std;

double
radial_basis(double r)
{
  if(r==0.0)
    return 0.0;
  return pow(r, 2) * log(r);
}

template<typename T1, typename T2, int Dim>
  double
  radialbasis(const boost::array<T1, Dim> & v1, const boost::array<T2, Dim> & v2)
  {
    boost::numeric::ublas::vector<double> v(Dim);
    for(unsigned int i(0); i < v.size(); ++i)
    {
      v(i) = v1[i] - v2[i];
    }
    return radial_basis(norm_2(v));
  }

template<int PosDim, int ValDim, typename WorkingType = double>
  class ThinPlateSpline
  {
  public:
    boost::numeric::ublas::matrix<WorkingType> Wa;
    std::vector<boost::array<WorkingType, PosDim> > refPositions;

    ThinPlateSpline()
    {}

    ThinPlateSpline(std::vector<boost::array<WorkingType, PosDim> > positions,
        std::vector<boost::array<WorkingType, ValDim> > values)
    {
      boost::numeric::ublas::matrix<WorkingType> L;

      refPositions = positions;

      const int numPoints((int)refPositions.size());
      const int WaLength(numPoints + PosDim + 1);

      L = boost::numeric::ublas::matrix<WorkingType> (WaLength, WaLength);

      // Calculate K and store in L
      for(int i(0); i < numPoints; i++)
      {
        L(i,i) = 0.0;
        // K is symmetrical so no point in calculating things twice
        int j(i + 1);
        for(; j < numPoints; ++j)
        {

          L(i,j) = L(j,i) = radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], refPositions[j]);
        }

        // construct P and store in K
        L(j,i) = L(i,j) = 1.0;
        ++j;
        for(int posElm(0); j < WaLength; ++posElm, ++j)
          L(j,i) = L(i,j) = positions[i][posElm];
      }

      // O
      for(int i(numPoints); i < WaLength; i++)
        for(int j(numPoints); j < WaLength; j++)
          L(i,j) = 0.0;

      // Solve L^-1 Y = W^T

      typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;

      boost::numeric::ublas::matrix<WorkingType> A(L);
      pmatrix pm(A.size1());
      int res = (int)lu_factorize(A, pm);
      if(res != 0)
	  {
        ;//TODO catch this error
	  }

      boost::numeric::ublas::matrix<WorkingType> invL(boost::numeric::ublas::identity_matrix<WorkingType>(A.size1()));
      lu_substitute(A, pm, invL);

      Wa = boost::numeric::ublas::matrix<WorkingType>(WaLength, ValDim );

      boost::numeric::ublas::matrix<WorkingType> Y(WaLength, ValDim);
      int i(0);
      for(; i < numPoints; i++)
        for(int j(0); j < ValDim; ++j)
          Y(i, j) = values[i][j];

      for(; i < WaLength; i++)
        for(int j(0); j < ValDim; ++j)
          Y(i, j) = 0.0;

      Wa = prod(invL,Y);

    }

    boost::array<WorkingType, ValDim>
    interpolate(const boost::array<WorkingType, PosDim> &position) const
    {
      boost::array<WorkingType, ValDim> result;
      // Init result
      for(int j(0); j < ValDim; ++j)
        result[j] = 0;

      unsigned int i(0);
      for(; i < Wa.size1() - (PosDim+1); ++i)
      {
        for(int j(0); j < ValDim; ++j)
          result[j] += Wa(i,j) * radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], position);
      }

      for(int j(0); j < ValDim; ++j)
        result[j] += Wa(i,j);
      ++i;

      for(int k(0); k < PosDim; ++k, ++i)
      {
        for(int j(0); j < ValDim; ++j)
          result[j] += Wa(i,j) * position[k];
      }
      return result;
    }

  };

#endif /* TPSINTERPOLATE_HPP_ */


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_COMPUTEROOTS_HPP
#define MCL_COMPUTEROOTS_HPP 1

#include <Eigen/Core>
//#include <cmath>
#include <complex>

namespace mcl
{

// Returns the smallest, positive, real root for quadratic
// a x^2 + b x + c
// Returns a negative value if no roots found
// from https://github.com/mike323zyf/BCQN
static inline double compute_quad_roots(double a, double b, double c, double tol=1e-16)
{
	double t = -1;
	if( std::abs(a) <= tol ){ t = -c / b; }
	else {
		double desc = b*b - 4.0 * a*c;
		if( desc > 0.0 ){
			t = (-b - std::sqrt(desc)) / (2.0 * a);
			if (t < 0.0){
				t = (-b + std::sqrt(desc)) / (2.0 * a);
			}
		}
		else{ // linear
			t = -b / (2.0*a);
		}
	}
	return t;
}

// Returns a negative value if no roots found
// from: https://github.com/libigl/libigl/blob/main/include/igl/flip_avoiding_line_search.cpp
static inline double compute_quad_roots_2(double a, double b, double c)
{
	double t1 = 0, t2 = 0;
	const double polyCoefEps = 1e-16;
	const double max_time = 2;
	if (abs(a) > polyCoefEps)
  {
		double delta_in = pow(b, 2) - 4 * a*c;
		if (delta_in < 0) {
			return -1;
		}
		double delta = sqrt(delta_in);
		t1 = (-b + delta) / (2 * a);
		t2 = (-b - delta) / (2 * a);
	}
	else if (abs(b) > polyCoefEps){
		 t1 = t2 = -c / b;
	} else {
		return -1;
	}
	if (t1 < 0) t1 = max_time;
	if (t2 < 0) t2 = max_time;

	if (!std::isfinite(t1) || !std::isfinite(t2))
		return -1;

	double tmp_n = std::min(t1, t2);
	t1 = std::max(t1, t2); t2 = tmp_n;
	if (t1 > 0) {
		if (t2 > 0) {
			return t2;
		}
		else {
			return t1;
		}
	}
	else {
		return -1;
	}
}

// Returns the smallest, positive, real root for cubic
// a x^3 + b x^2 + c x + d
// Returns a negative value if no roots found
// from https://github.com/mike323zyf/BCQN
static inline double compute_cubic_roots(double a, double b, double c, double d, double tol=1e-16)
{
	double t = -1;
	if(std::abs(a) <= tol){ t = compute_quad_roots(b, c, d, tol); }
	else {
		std::complex<double> i(0, 1);
		std::complex<double> delta0(b*b - 3 * a*c, 0);
		std::complex<double> delta1(2 * b*b*b - 9 * a*b*c + 27 * a*a*d, 0);
		std::complex<double> C = pow((delta1 + sqrt(delta1*delta1 - 4.0 * delta0*delta0*delta0)) / 2.0, 1.0 / 3.0);

		std::complex<double> u2 = (-1.0 + sqrt(3.0)*i) / 2.0;
		std::complex<double> u3 = (-1.0 - sqrt(3.0)*i) / 2.0;

		std::complex<double> t1 = (b + C + delta0 / C) / (-3.0*a);
		std::complex<double> t2 = (b + u2*C + delta0 / (u2*C)) / (-3.0*a);
		std::complex<double> t3 = (b + u3*C + delta0 / (u3*C)) / (-3.0*a);

		if ((std::abs(std::imag(t1))<tol) && (std::real(t1)>0))
			t = std::real(t1);
		if ((std::abs(std::imag(t2))<tol) && (std::real(t2)>0) && ((std::real(t2) < t) || (t < 0)))
			t = std::real(t2);
		if ((std::abs(std::imag(t3))<tol) && (std::real(t3)>0) && ((std::real(t3) < t) || (t < 0)))
			t = std::real(t3);
	}
	return t;
}

// src: https://github.com/libigl/libigl/blob/main/include/igl/flip_avoiding_line_search.cpp
static inline double compute_min_pos_root_2D(const Eigen::MatrixXd& x, const Eigen::MatrixXi& E, Eigen::MatrixXd& p, int f)
{
	int v1 = E(f,0);
	int v2 = E(f,1);
	int v3 = E(f,2);
	const double& U11 = x(v1,0);
	const double& U12 = x(v1,1);
	const double& U21 = x(v2,0);
	const double& U22 = x(v2,1);
	const double& U31 = x(v3,0);
	const double& U32 = x(v3,1);
	const double& V11 = p(v1,0);
	const double& V12 = p(v1,1);
	const double& V21 = p(v2,0);
	const double& V22 = p(v2,1);
	const double& V31 = p(v3,0);
	const double& V32 = p(v3,1);
	double a = V11*V22 - V12*V21 - V11*V32 + V12*V31 + V21*V32 - V22*V31;
	double b = U11*V22 - U12*V21 - U21*V12 + U22*V11 - U11*V32 + U12*V31 + U31*V12 - U32*V11 + U21*V32 - U22*V31 - U31*V22 + U32*V21;
	double c = U11*U22 - U12*U21 - U11*U32 + U12*U31 + U21*U32 - U22*U31;
  double root = compute_quad_roots_2(a,b,c);
  if (root < 0) { return std::numeric_limits<float>::max(); }
  return root;
}

} // end mcl

#endif


#include <iostream>
#include <Eigen/Dense>

void testEuclideanNorm()
{
  Eigen::Matrix<double, 3, 5> A;
  A << 1, 2, 3, 4, 5,
       2, 3, 4, 5, 6,
       7, 9, 10, 11, 12;

  std::cout << A.colwise().norm() << std::endl << std::endl;

  Eigen::Matrix<double, 3, 5> B;
  B = A.array().rowwise() / A.colwise().norm().array();
  std::cout << B << std::endl;

  for(int i=0; i<A.cols(); ++i)
  {
    Eigen::Vector3d b = A.col(i)/A.col(i).norm();
    std::cout << b.transpose() << std::endl;
    std::cout << b.norm() << std::endl;
  }

}

int main(int argc, char** argv)
{
  testEuclideanNorm();
  return 0;
}


/**
 * @ file pointEvaluation.cc
 * @ brief NPDE homework PointEvaluationRhs code
 * @ author Christian Mitsch, Liaowang Huang (refactoring)
 * @ date 22/03/2019, 06/01/2020 (refactoring)
 * @ copyright Developed at ETH Zurich
 */

#include "pointevaluationrhs.h"

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SparseLU>
#include <cmath>
#include <utility>

#include "pointevaluationrhs_norms.h"

namespace PointEvaluationRhs {

/* SAM_LISTING_BEGIN_5 */
Eigen::Vector2d GlobalInverseTria(Eigen::Matrix<double, 2, 3> mycorners,
                                  Eigen::Vector2d x) {
  Eigen::Vector2d x_hat;

  //====================
  // Your code goes here
  //====================
  return x_hat;
}
/* SAM_LISTING_END_5 */

/** @brief Numerically stable solution of a quadratic equation in R
 * @param a,b,c coefficients of quadratic polynomias ax^2+bx+c
 * @return both zeros, NaN if complex
 */
/* SAM_LISTING_BEGIN_4 */
std::pair<double, double> solveQuadraticEquation(double a, double b, double c) {
  // Implement the cases which are solvable and return their solutions
  //====================
  // Your code goes here
  //====================
  // Return NAN if there are no (real) roots
  return {NAN, NAN};
}

/* SAM_LISTING_END_4 */

/** @brief Computes the area of a triangle
 * @param a,b,c vertex coordinate vectors
 */
inline double triaArea(const Eigen::Vector2d a, const Eigen::Vector2d b,
                       const Eigen::Vector2d c) {
  double result = 0;
  //====================
  // Your code goes here
  //====================
  return result;
}

Eigen::Vector2d GlobalInverseQuad(Eigen::Matrix<double, 2, 4> vert,
                                  Eigen::Vector2d x) {
  constexpr double kEPS = 1.0E-8;
  Eigen::Vector2d x_hat;

  // Implement and use the functions triaArea and solveQuadraticEquation
  //====================
  // Your code goes here
  //====================
  return x_hat;
}

std::pair<double, double> normsSolutionPointLoadDirichletBVP(
    const lf::assemble::DofHandler &dofh, Eigen::Vector2d source_point,
    Eigen::VectorXd &sol_vec) {
  std::pair<double, double> result(0, 0);
  const unsigned int N_dofs = dofh.NumDofs();
  sol_vec.resize(N_dofs);
  sol_vec.setZero();
  //====================
  // Your code goes here
  //====================
  return result;
}

/* SAM_LISTING_BEGIN_6 */
Eigen::VectorXd DeltaLocalVectorAssembler::Eval(const lf::mesh::Entity &cell) {
  Eigen::VectorXd result;
  // get the coordinates of the corners of this cell
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  auto vertices = lf::geometry::Corners(*geo_ptr);
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_6 */

}  // namespace PointEvaluationRhs


/*
 * Website:
 *      https://github.com/wo3kie/dojo
 *
 * Author:
 *      Lukasz Czerwinski
 *
 * Compilation:
 *      g++ --std=c++11 interpolation.cpp -o interpolation -lgsl -lgslcblas -lm
 *
 * Usage:
 *      $ ./interpolation
 */

#include <algorithm>
#include <vector>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <gsl/gsl_linalg.h>

#include "./feq.hpp"

namespace ublas = boost::numeric::ublas;

struct Point
{
    double x_;
    double y_;
};

ublas::vector< double > linear_interpolation_ublas(
    std::vector< Point > const & points
){
    assert( points.size() == 2 );

    ublas::vector< double > X( 2 );

    Point const & p1 = points[ 0 ];
    Point const & p2 = points[ 1 ];

    double const a = (p1.y_ - p2.y_) / (p1.x_ - p2.x_);
    double const b = p1.y_ - p1.x_ * ((p1.y_ - p2.y_) / (p1.x_ - p2.x_));

    X( 0 ) = a;
    X( 1 ) = b;

    return X;
}

ublas::vector< double > polynominal_interpolation_ublas( std::vector< Point > const & points ){
    std::size_t const size = points.size();

    ublas::matrix< double > A( size, size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        for( std::size_t j = 0 ; j < size ; ++ j ){
            A( i, j ) = std::pow( points[ i ].x_, j );
        }
    }

    ublas::vector< double > Y( size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        Y( i ) = points[ i ].y_;
    }

    ublas::permutation_matrix< double > PM( size );
    ublas::lu_factorize( A, PM );
    ublas::lu_substitute( A, PM, Y );

    return Y;
}

gsl_vector * linear_interpolation_gsl( std::vector< Point > const & points ){
    assert( points.size() == 2 );

    gsl_vector * X = gsl_vector_alloc( 2 );

    Point const & p1 = points[ 0 ];
    Point const & p2 = points[ 1 ];

    double const a = (p1.y_ - p2.y_) / (p1.x_ - p2.x_);
    double const b = p1.y_ - p1.x_ * ((p1.y_ - p2.y_) / (p1.x_ - p2.x_));

    gsl_vector_set( X, 0, a );
    gsl_vector_set( X, 1, b );

    return X;
}

gsl_vector * polynominal_interpolation_gsl( std::vector< Point > const & points ){
    std::size_t const size = points.size();

    gsl_matrix * A = gsl_matrix_alloc( size, size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        for( std::size_t j = 0 ; j < size ; ++ j ){
            gsl_matrix_set( A, i, j, std::pow( points[ i ].x_, j ) );
        }
    }

    gsl_vector * Y = gsl_vector_alloc( size );

    for( std::size_t i = 0 ; i < size ; ++ i ){
        gsl_vector_set( Y, i, points[ i ].y_ );
    }

    gsl_vector * X = gsl_vector_alloc( size );
    gsl_permutation * PM = gsl_permutation_alloc( size );

    int s = 0;
    gsl_linalg_LU_decomp( A, PM, &s );
    gsl_linalg_LU_solve( A, PM, Y, X );

    gsl_permutation_free( PM );
    gsl_vector_free( Y );
    gsl_matrix_free( A );

    return X;
}

void linear_interpolation_ublas_test(){
    ublas::vector< double > expected( 2 );
    expected( 0 ) = 5.0 / 2;
    expected( 1 ) = -13.0 / 2;

    ublas::vector< double > const actual = linear_interpolation_ublas(
        std::vector< Point >{ { 5, 6 }, { 7, 11 } }
    );

    auto feqDouble = [](double d1, double d2){
        return feq(d1, d2);
    };

    assert(
        std::equal(
            actual.begin(),
            actual.end(),
            expected.begin(),
            feqDouble
        )
    );
}

void polynominal_interpolation_ublas_test(){
    ublas::vector< double > expected( 3 );
    expected( 0 ) = 7;
    expected( 1 ) = 6;
    expected( 2 ) = -1;

    ublas::vector< double > const actual = polynominal_interpolation_ublas(
        std::vector< Point >{ { 1, 12 }, { 2, 15 }, { 3, 16 } }
    );

    auto feqDouble = [](double d1, double d2){
        return feq(d1, d2);
    };

    assert(
        std::equal(
            actual.begin(),
            actual.end(),
            expected.begin(),
            feqDouble
        )
    );
}

void linear_interpolation_gsl_test(){
    gsl_vector * expected = gsl_vector_alloc( 2 );
    gsl_vector_set( expected, 0, 5.0 / 2 );
    gsl_vector_set( expected, 1, -13.0 / 2 );

    gsl_vector * actual = linear_interpolation_gsl(
        std::vector< Point >{ { 5, 6 }, { 7, 11 } }
    );

    for( std::size_t i = 0 ; i < 2 ; ++ i ){
        assert((
            feq(
                gsl_vector_get( actual, i ),
                gsl_vector_get( expected, i )
            )
        ));
    }

    gsl_vector_free( actual );
    gsl_vector_free( expected );
}

void polynominal_interpolation_gsl_test(){
    gsl_vector * expected = gsl_vector_alloc( 3 );
    gsl_vector_set( expected, 0, 7 );
    gsl_vector_set( expected, 1, 6 );
    gsl_vector_set( expected, 2, -1 );

    gsl_vector * actual = polynominal_interpolation_gsl(
        std::vector< Point >{ { 1, 12 }, { 2, 15 }, { 3, 16 } }
    );

    for( std::size_t i = 0 ; i < 3 ; ++ i ){
        assert((
            feq(
                gsl_vector_get( actual, i ),
                gsl_vector_get( expected, i )
            )
        ));
    }

    gsl_vector_free( actual );
    gsl_vector_free( expected );
}

#include <vector>

int main(){
    linear_interpolation_ublas_test();
    linear_interpolation_gsl_test();

    polynominal_interpolation_ublas_test();
    polynominal_interpolation_gsl_test();
}


/*
 * Website:
 *      https://github.com/wo3kie/dojo
 *
 * Author:
 *      Lukasz Czerwinski
 *
 * Compilation:
 *      g++ --std=c++11 bond.cpp -o bond -lboost_date_time
 *
 * Usage:
 *      $ ./bond 2015/12/31 100 0.08 0.08 2014/12/31
 *        PV( F ): 92.5926
 *        PV( Coupon ): 7.40741
 *      P: 100
 */

/*
 *                                                                      Maturity Date
 *                                                                       |
 *                     /-- 1 year -----------    -- 1 year------------\  |
 *                    /                      \  /                      \ |
 *                   v                        \v                        \v
 * ... - 2 - 4 - 6 - 8 - 10 - 12 - 2 - 4 - 6 - 8 - 10 - 12 - 2 - 4 - 6 - 8 - 10 - ...
 *       ^           ^                         ^                         ^
 *       |           |                         |                         |
 *       |          Coupon                    Coupon                    Face Value
 *      Price Day                                                       Coupon
 */

#include <cmath>
#include <iostream>

#include <boost/date_time/gregorian/gregorian.hpp>

namespace date = boost::gregorian;
namespace date_time = boost::date_time;

double bondPrice(
    date::date maturityDate,
    double faceValue,
    double yield,
    double intrestRate,
    date::date priceDay
)
{
    using std::pow;

    int days = ( maturityDate - priceDay ).days();

    if( days < 0 ){
        return 0;
    }

    double result = 0;

    {
        /*
         * Face value
         */

        result = faceValue / pow( ( 1.0 + intrestRate ), 1.0 * days / 365 );

        std::cout << "  PV( F ): " << result << std::endl;
    }

    while( days > 0 )
    {
        /*
         * Coupons
         */

        double coupon =
            ( faceValue * yield ) / pow( ( 1.0 + intrestRate ), 1.0 * days / 365 );

        std::cout << "  PV( Coupon ): " << coupon << std::endl;

        result += coupon;

        maturityDate = maturityDate - date::years( 1 );

        days = ( maturityDate - priceDay ).days();
    }

    std::cout << "P: " << result << std::endl;

    return result;
}

int main( int argc, char* argv[] )
{
    if( argc != 6 )
    {
        std::cerr << "Usage: " << argv[0] << " maturityDate faceValue yield intrestRate priceDate" << std::endl;
        std::cerr << "       " << argv[0] << " 2015/12/31 100 0.08 0.08 2014/12/31" << std::endl;

        return 1;
    }

    bondPrice(
        date::from_string( argv[1] ),
        std::stoi( argv[2] ),
        std::stof( argv[3] ),
        std::stof( argv[4] ),
        date::from_string( argv[5] )
    );

    return 0;
}


#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>
#include "coordinate_transform.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <functional>

//----------------compVectorBegin----------------
//! Evaluate the load vector on the triangle spanned by
//! the points (a, b, c).
//!
//! Here, the load vector is a vector $(v_i)$ of
//! three components, where
//!
//! $$v_i = \int_{K} \lambda_i^K(x, y) f(x, y) \; dV$$
//!
//! where $K$ is the triangle spanned by (a, b, c).
//!
//! @param[out] loadVector should be a vector of length 3.
//!                        At the end, will contain the integrals above.
//!
//! @param[in] a the first corner of the triangle
//! @param[in] b the second corner of the triangle
//! @param[in] c the third corner of the triangle
//! @param[in] f the function f (LHS).
template<class Vector, class Point>
void computeLoadVector(Vector& loadVector,
                    const Point& a, const Point& b, const Point& c,
                    const std::function<double(double, double)>& f)
{
    Eigen::Matrix2d coordinateTransform = makeCoordinateTransform(b - a, c - a);
    double volumeFactor = std::abs(coordinateTransform.determinant());
// (write your solution here)

}
//----------------compVectorEnd----------------


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__MATH_HPP_
#define CBR_MATH__MATH_HPP_

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

#include <algorithm>
#include <array>
#include <cmath>
#include <exception>
#include <limits>
#include <type_traits>

namespace cbr
{

/***************************************************************************
 * \brief Converts degrees to radians
 ***************************************************************************/
template<typename T>
constexpr T deg2rad(const T deg) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return deg * M_PI / 180.;
}

/***************************************************************************
 * \brief Converts radians to degrees
 ***************************************************************************/
template<typename T>
constexpr T rad2deg(const T rad) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return rad * 180. / M_PI;
}

/***************************************************************************
 * \brief Computes b^e
 ***************************************************************************/
template<typename TBase, typename TExp>
constexpr TBase powFast(const TBase b, const TExp e) noexcept
{
  static_assert(std::is_unsigned_v<TExp>, "Exponent type must be unsigned.");

  if (e == 0) {
    return TBase(1.);
  }

  if (e == 1) {
    return b;
  }

  TBase out = b;
  for (TExp i = 2; i <= e; i++) {
    out *= b;
  }

  return out;
}

namespace detail
{

template<typename T, std::size_t ... Is>
constexpr T powFastImpl([[maybe_unused]] T val, std::index_sequence<Is...>)
{
  #pragma GCC diagnostic push
  #pragma GCC diagnostic ignored "-Wunused-value"
  #pragma GCC diagnostic ignored "-Wconversion"
  // *INDENT-OFF*
  return ((Is, val) * ... * T{1});
  // *INDENT-ON*
  #pragma GCC diagnostic pop
}

}  // namespace detail

// /***************************************************************************
//  * \brief Computes b^e via expansion to e-length product  b * ... * b
//  ***************************************************************************/
template<std::size_t e = 2, typename TBase>
constexpr TBase powFast(const TBase b) noexcept
{
  return detail::powFastImpl(b, std::make_index_sequence<e>{});
}

/***************************************************************************
* \brief Wraps angle in radians between 0 and 2*pi
***************************************************************************/
template<typename T>
T wrap2Pi(const T in) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return in - std::floor(in / T(2. * M_PI)) * T(2. * M_PI);
}

/***************************************************************************
* \brief Wraps angle in radians between -pi and pi
***************************************************************************/
template<typename T>
T wrapPi(const T in) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return wrap2Pi(in + M_PI) - M_PI;
}

/***************************************************************************
 * \brief Converts yaw in radians to heading in degrees
 ***************************************************************************/
template<typename T>
T yaw2heading(const T rad) noexcept
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return rad2deg(wrap2Pi(-rad));
}

/***************************************************************************
 * \brief Computes truncated factorial : product from x0(default: 1) to x.
 ***************************************************************************/
template<typename T>
constexpr T factorial(const T x, const T x0 = T(1)) noexcept
{
  static_assert(std::is_unsigned_v<T>, "Type must be unsigned.");

  if (x == 0 || x0 > x) {
    return T(1);
  }

  if (x0 >= x) {
    return x;
  }

  T out = x0;
  for (T i = x0 + T(1); i <= x; i++) {
    out *= i;
  }
  return out;
}

/***************************************************************************
 * \brief Converts probability to log odds
 ***************************************************************************/
template<typename T>
constexpr T prob_to_log_odds(const T p)
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return log(p / (1. - p));
}

/***************************************************************************
 * \brief Converts to log odds to probability
 ***************************************************************************/
template<typename T>
constexpr T log_odds_to_prob(const T l)
{
  static_assert(std::is_floating_point_v<T>, "Input type must be a floating point.");
  return 1. - (1. / (1. + exp(l)));
}

/***************************************************************************
 * \brief Converts a quaternion to euler angles using the ZYX decomposition
 * The returned vector contains the angles in [roll, pitch, yaw] order
 ***************************************************************************/
template<typename derived>
Eigen::Matrix<typename derived::Scalar, 3, 1>
quat2eulZYX(const Eigen::QuaternionBase<derived> & q) noexcept
{
  const auto qn = q.normalized();
  using T = typename derived::Scalar;
  Eigen::Matrix<T, 3, 1> eul;

  // roll
  eul[0] = atan2(
    T(2.) * (qn.w() * qn.x() + qn.y() * qn.z()),
    T(1.) - T(2.) * (powFast<2>(qn.x()) + powFast<2>(qn.y())));

  // pitch
  const T sinp = T(2.) * (qn.w() * qn.y() - qn.z() * qn.x());
  // Autodiff friendly manner of handling numerical errors
  if (abs(sinp) >= T(1.)) {
    if (sinp > T(0.)) {
      eul[1] = T(M_PI_2);
    } else {
      eul[1] = -T(M_PI_2);
    }
  } else {
    eul[1] = asin(sinp);
  }

  // yaw
  eul[2] = atan2(
    T(2.) * (qn.w() * qn.z() + qn.x() * qn.y()),
    T(1.) - T(2.) * (powFast<2>(qn.y()) + powFast<2>(qn.z())));

  return eul;
}

/***************************************************************************
 * \brief Converts a quaternion to euler angles using the ZYX decomposition
 * The returned vector contains the angles in [roll, pitch, yaw] order
 ***************************************************************************/
template<typename derived>
void quat2eulZYX(
  const Eigen::QuaternionBase<derived> & q,
  Eigen::Matrix<typename derived::Scalar, 3, 1> & eul) noexcept
{
  eul = quat2eulZYX<derived>(q);
}

/***************************************************************************
 * \brief Converts euler angles to a quaternion using the ZYX decomposition
 * The input vector must contain the angles in [roll, pitch, yaw] order
 ***************************************************************************/
template<typename T>
Eigen::Quaternion<T> eul2quatZYX(const Eigen::Matrix<T, 3, 1> & eul) noexcept
{
  Eigen::Quaternion<T> q;

  T cy = cos(eul[2] * .5);
  T sy = sin(eul[2] * .5);
  T cp = cos(eul[1] * .5);
  T sp = sin(eul[1] * .5);
  T cr = cos(eul[0] * .5);
  T sr = sin(eul[0] * .5);

  q.w() = cy * cp * cr + sy * sp * sr;
  q.x() = cy * cp * sr - sy * sp * cr;
  q.y() = sy * cp * sr + cy * sp * cr;
  q.z() = sy * cp * cr - cy * sp * sr;

  return q;
}

template<typename T, typename derived>
void eul2quatZYX(
  const Eigen::Matrix<T, 3, 1> & eul,
  Eigen::QuaternionBase<derived> & q) noexcept
{
  q = eul2quatZYX<T>(eul);
}

/***************************************************************************
 * \brief Converts a pure yaw to a quaternion
 ***************************************************************************/
template<typename T>
Eigen::Quaternion<T> yaw2quat(const T & yaw) noexcept
{
  return Eigen::Quaternion<T>(cos(yaw * T(.5)), T(0.0), T(0.0), sin(yaw * T(.5)));
}

/***************************************************************************
 * \brief Extract the yaw component of a quaternion
 ***************************************************************************/
template<typename derived>
typename derived::Scalar quat2yaw(const Eigen::QuaternionBase<derived> & q) noexcept
{
  using T = typename derived::Scalar;

  const auto qn = q.normalized();
  return atan2(
    T(2.) * (qn.w() * qn.z() + qn.x() * qn.y()),
    T(1.) - T(2.) * (powFast<2>(qn.y()) + powFast<2>(qn.z())));
}

/***************************************************************************
 * \brief Convert subscripts to linear indices
 ***************************************************************************/
template<typename T, std::size_t N>
constexpr T sub2ind(const std::array<T, N> & sz, const std::array<T, N> & idx)
{
  static_assert(std::is_arithmetic_v<T>, "Type must be arithmetic.");
  static_assert(N > 1, "Size of inputs must be > 1.");

  T ind = idx[0] + idx[1] * sz[0];

  if constexpr (N > 2) {
    T stride = sz[0];
    for (std::size_t i = 2; i < N; i++) {
      stride *= sz[i - 1];
      ind += idx[i] * stride;
    }
  }

  return ind;
}

/***************************************************************************
 * \brief Convert linear indices to subscripts
 ***************************************************************************/
template<typename T, std::size_t N>
constexpr std::array<T, N> ind2sub(const std::array<T, N> & sz, T idx)
{
  static_assert(std::is_arithmetic_v<T>, "Type must be arithmetic.");
  static_assert(N > 1, "Size of first input must be > 1.");

  std::array<T, N> sub{};

  if constexpr (N > 2) {
    std::array<T, N - 2> strides{};
    strides[0] = sz[0] * sz[1];
    for (std::size_t i = 1; i < N - 2; i++) {
      strides[i] = strides[i - 1] * sz[i + 1];
    }
    for (std::size_t i = N - 1; i > 1; i--) {
      sub[i] = idx / strides[i - 2];
      idx = idx % strides[i - 2];
    }
  }

  sub[1] = idx / sz[0];
  sub[0] = idx % sz[0];

  return sub;
}

/***************************************************************************
 * \brief Antisymmetric power function
 ***************************************************************************/
template<typename T1, typename T2>
auto powAntisym(T1 base, T2 exp)
{
  static_assert(std::is_arithmetic_v<T1>, "Base type must be arithmetic.");
  static_assert(std::is_arithmetic_v<T2>, "Exponent type must be arithmetic.");

  if (base >= T1(0)) {
    return std::pow(base, exp);
  } else {
    return -std::pow(-base, exp);
  }
}

/***************************************************************************
 * \brief Over-approximate a body bounding box with a world bounding box
 ***************************************************************************/
template<typename S, typename T, int I>
Eigen::AlignedBox<T, I> overapp_bbox(S && pose, Eigen::AlignedBox<T, I> bbox_B)
{
  Eigen::Array<T, I, 1> bbmin = std::numeric_limits<T>::max() * Eigen::Array<T, I, 1>::Ones();
  Eigen::Array<T, I, 1> bbmax = std::numeric_limits<T>::min() * Eigen::Array<T, I, 1>::Ones();
  for (int n = 0; n != pow(2, I); ++n) {  // find min/max coordinates of bounding box in world frame
    auto p = pose * bbox_B.corner(static_cast<typename Eigen::AlignedBox<T, I>::CornerType>(n));
    bbmin = bbmin.min(p.array());
    bbmax = bbmax.max(p.array());
  }
  return Eigen::AlignedBox<T, I>{bbmin, bbmax};
}

/***************************************************************************
 * \brief Smooth saturation function
 ***************************************************************************/
template<typename T1, typename T2>
constexpr void smoothSatInPlace(
  T1 & x,
  const T2 & mi,
  const T2 & ma,
  const T2 & satSmoothCoeff = T2(0.1))
{
  static_assert(std::is_floating_point_v<T2>, "Input type must be a floating point.");

  if (satSmoothCoeff < T2(0.) || T2(1.) < satSmoothCoeff) {
    throw std::invalid_argument("satSmoothCoeff must be between 0 and 1.");
  }
  if (ma < mi) {
    throw std::invalid_argument("max must be greated or equal to min.");
  }

  constexpr T2 alpha = M_PI / 8.;
  constexpr T2 beta = M_PI / 4.;
  const T2 r = satSmoothCoeff * M_SQRT2 / tan(alpha);
  const T2 bevelL = r * tan(alpha);
  const T2 bevelStart = 1. - cos(beta) * bevelL;
  const T2 bevelStop = 1. + bevelL;
  const T2 bevelXc = bevelStop;
  const T2 bevelYc = 1. - r;

  const T2 range = ma - mi;
  const T2 middle = (ma + mi) / 2.;
  const T1 uc = 2. * (x - middle) / range;

  if (uc >= bevelStop) {
    x = ma;
  } else if (uc <= -bevelStop) {
    x = mi;
  } else if (uc > bevelStart) {
    x = 0.5 * (sqrt(r * r - (uc - bevelXc) * (uc - bevelXc)) + bevelYc) * range + middle;
  } else if (uc < -bevelStart) {
    x = 0.5 * (-sqrt(r * r - (uc + bevelXc) * (uc + bevelXc)) - bevelYc) * range + middle;
  } else {
    return;
  }
}

template<typename T1, typename T2>
constexpr T1 smoothSat(
  const T1 & x,
  const T2 & mi,
  const T2 & ma,
  const T2 & satSmoothCoeff = T2{0.1})
{
  T1 out{x};
  smoothSatInPlace<T1, T2>(out, mi, ma, satSmoothCoeff);
  return out;
}

/***************************************************************************
 * \brief Check if point is inside polygon
 ***************************************************************************/
template<typename ForwardIterator, typename Point>
bool point_in_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const Point & point)
{
  ForwardIterator current = first;
  if (current == last) {return false;}  // empty list of vertices

  ForwardIterator next = current; ++next;
  if (next == last) {return false;}  // only 1 vertex

  ForwardIterator next_plus_1 = next; ++next_plus_1;
  if (next_plus_1 == last) {return false;}  // only 2 vertex

  auto which_side_in_slab =
    [](
    const auto & pt,
    const auto & low,
    const auto & high) -> int
    {
      const double cross_product =
        (high[0] - low[0]) * (pt[1] - low[1]) - (high[1] - low[1]) * (pt[0] - low[0]);

      if (cross_product > 0.) {
        return 1;
      }

      if (cross_product < 0.) {
        return -1;
      }

      return 0;
    };

  auto compare_x_2 = [](const auto & p1, const auto & p2) -> int {
      if (p1[0] < p2[0]) {
        return -1;
      } else if (p1[0] > p2[0]) {
        return 1;
      }
      return 0;
    };

  auto compare_y_2 = [](const auto & p1, const auto & p2) -> int {
      if (p1[1] < p2[1]) {
        return -1;
      } else if (p1[1] > p2[1]) {
        return 1;
      }
      return 0;
    };

  bool IsInside = false;
  int cur_y_comp_res = compare_y_2(*current, point);

  do {
    int next_y_comp_res = compare_y_2(*next, point);

    switch (cur_y_comp_res) {
      case -1:
        switch (next_y_comp_res) {
          case -1:
            break;
          case 0:
            switch (compare_x_2(point, *next)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
              case 1:
                break;
            }
            break;
          case 1:
            switch (which_side_in_slab(point, *current, *next)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
            }
            break;
        }
        break;
      case 0:
        switch (next_y_comp_res) {
          case -1:
            switch (compare_x_2(point, *current)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
              case 1:
                break;
            }
            break;
          case 0:
            switch (compare_x_2(point, *current)) {
              case -1:
                if (compare_x_2(point, *next) != -1) {
                  return true;
                }
                break;
              case 0:
                return true;
              case 1:
                if (compare_x_2(point, *next) != 1) {
                  return true;
                }
                break;
            }
            break;
          case 1:
            if (compare_x_2(point, *current) == 0) {
              return true;
            }
            break;
        }
        break;
      case 1:
        switch (next_y_comp_res) {
          case -1:
            switch (which_side_in_slab(point, *next, *current)) {
              case -1:
                {IsInside = !IsInside; break;}
              case 0:
                return true;
            }
            break;
          case 0:
            if (compare_x_2(point, *next) == 0) {
              return true;
            }
            break;
          case 1:
            break;
        }
        break;
    }
    current = next;
    cur_y_comp_res = next_y_comp_res;
    ++next;
    if (next == last) {next = first;}
  } while (current != first);

  return IsInside;
}

/***************************************************************************
 * \brief Compute distance from point to segment
 ***************************************************************************/
template<typename Point_t>
constexpr auto point_to_segment_dist_squared(
  const Point_t & pt,
  const Point_t & seg_pt1,
  const Point_t & seg_pt2)
{
  using scalar_t = std::decay_t<decltype(pt[0])>;

  const scalar_t pt_x = pt[0] - seg_pt1[0];
  const scalar_t pt_y = pt[1] - seg_pt1[1];

  if (seg_pt1[0] == seg_pt2[0] && seg_pt1[1] == seg_pt2[1]) {
    return pt_x * pt_x + pt_y * pt_y;
  }

  const scalar_t seg_x = seg_pt2[0] - seg_pt1[0];
  const scalar_t seg_y = seg_pt2[1] - seg_pt1[1];

  const scalar_t l2 = seg_x * seg_x + seg_y * seg_y;
  const scalar_t proj = (pt_x * seg_x + pt_y * seg_y) / l2;
  const scalar_t t = std::min(std::max(proj, scalar_t(0.)), scalar_t(1.));

  const scalar_t dist_x = t * seg_x - pt_x;
  const scalar_t dist_y = t * seg_y - pt_y;

  return dist_x * dist_x + dist_y * dist_y;
}

/***************************************************************************
 * \brief Checks if 2 segments intersect
 ***************************************************************************/
template<typename Point_t>
constexpr bool segments_intersect(
  const Point_t & seg1_pt1,
  const Point_t & seg1_pt2,
  const Point_t & seg2_pt1,
  const Point_t & seg2_pt2)
{
  if (seg1_pt1[0] == seg1_pt2[0] && seg1_pt1[1] == seg1_pt2[1]) {
    throw std::invalid_argument("First segment has no length");
  }

  if (seg2_pt1[0] == seg2_pt2[0] && seg2_pt1[1] == seg2_pt2[1]) {
    throw std::invalid_argument("Second segment has no length");
  }
  using scalar_t = std::decay_t<decltype(seg1_pt1[0])>;

  const scalar_t seg1_x = seg1_pt2[0] - seg1_pt1[0];
  const scalar_t seg1_y = seg1_pt2[1] - seg1_pt1[1];

  const scalar_t seg2_x = seg2_pt2[0] - seg2_pt1[0];
  const scalar_t seg2_y = seg2_pt2[1] - seg2_pt1[1];

  const scalar_t det = seg1_y * seg2_x - seg1_x * seg2_y;
  if (det == scalar_t(0.)) {
    return false;
  }

  const scalar_t pt1_dx = seg2_pt1[0] - seg1_pt1[0];
  const scalar_t pt1_dy = seg2_pt1[1] - seg1_pt1[1];

  const scalar_t alpha = (pt1_dy * seg2_x - pt1_dx * seg2_y) / det;
  const scalar_t beta = (pt1_dy * seg1_x - pt1_dx * seg1_y) / det;

  if (
    alpha < scalar_t(0.) ||
    alpha > scalar_t(1.) ||
    beta < scalar_t(0.) ||
    beta > scalar_t(1.))
  {
    return false;
  }

  return true;
}

template<typename PointPair>
bool segments_intersect(
  const PointPair & seg1,
  const PointPair & seg2)
{
  return segments_intersect(seg1.first, seg1.second, seg2.first, seg2.second);
}

/***************************************************************************
 * \brief Checks if segment intersects a polygon
 ***************************************************************************/
template<typename ForwardIterator, typename Point>
bool segment_inter_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const Point & point1,
  const Point & point2)
{
  ForwardIterator current = first;
  ForwardIterator next = current; ++next;

  do {
    if (segments_intersect(point1, point2, *current, *next)) {
      return true;
    }

    current = next;
    ++next;
    if (next == last) {next = first;}
  } while (current != first);

  return false;
}

template<typename ForwardIterator, typename PointPair>
bool segment_inter_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const PointPair & seg)
{
  return segment_inter_polygon(first, last, seg.first, seg.second);
}

/***************************************************************************
 * \brief Checks if segment is in the interior of a polygon
 ***************************************************************************/
template<typename ForwardIterator, typename Point>
bool segment_in_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const Point & point1,
  const Point & point2)
{
  if (!point_in_polygon(first, last, point1)) {
    return false;
  }

  return !segment_inter_polygon(first, last, point1, point2);
}

template<typename ForwardIterator, typename PointPair>
bool segment_in_polygon(
  ForwardIterator first,
  ForwardIterator last,
  const PointPair & seg)
{
  return segment_in_polygon(first, last, seg.first, seg.second);
}

/***************************************************************************
 * \brief Array of N evenly spaced numbers
 ***************************************************************************/
template<std::size_t N, typename T>
constexpr std::array<T, N> linspace(T x0, T xT)
{
  static_assert(std::is_floating_point_v<T>, "T must be floating-point number");
  static_assert(N >= 2, "N must be greater or equal to 2");

  T dx = (xT - x0) / static_cast<T>(N - 1);
  std::array<T, N> ret{x0};
  for (std::size_t i = 1; i != N; ++i) {
    ret[i] = x0 += dx;
  }
  return ret;
}

/***************************************************************************
 * \brief Sigmoid with center and steepness
 ***************************************************************************/
template<typename T>
T sigmoid(T x, T center, T k)
{
  static_assert(std::is_floating_point_v<T>, "T must be an arithmetic type");
  using std::exp;

  return T(1) / (T(1) + exp(-k * (x - center)));
}

namespace detail
{
constexpr Eigen::StorageOptions layout(size_t N)
{
  if (N > 1) {
    return Eigen::RowMajor;
  }
  return Eigen::ColMajor;
}
}   // namespace detail

/**
 * Calculate sample error covariance matrix \Sigma for a dataset and an error functions
 * @param data container with samples of type X, e.g. std::vector<X>
 * @param fcn mapping X -> ErrT where ErrT is an Eigen column vector/array of size D
 * @return covariance matrix of size D x D
 *
 *  \Sigma = (1/N-1) * \sum_i (fcn(x_i) - \bar x) * (fcn(x_i) - \bar x).transpose()
 *
 *   where  \bar x = (1/N) * \sum_i fcn(x_i)
 *
 */
template<typename DataContainerT, typename ErrorFcnT>
auto sample_covariance(
  const DataContainerT & data,
  ErrorFcnT && fcn = [](const auto & x) {return x;})
{
  using ResT = typename std::result_of_t<
    ErrorFcnT(typename DataContainerT::value_type)
    >::PlainMatrix;
  static_assert(ResT::ColsAtCompileTime == 1, "fcn must map to column vector");
  static_assert(std::is_base_of_v<Eigen::MatrixBase<ResT>, ResT>, "fcn must map to eigen type");

  static constexpr int N = ResT::RowsAtCompileTime;
  using InfT = Eigen::Matrix<typename ResT::Scalar, N, N>;

  InfT cov = InfT::Zero();

  if (data.size() > 1) {
    using CompT = Eigen::Matrix<
      typename ResT::Scalar, Eigen::Dynamic, N, detail::layout(N)
    >;
    CompT errs(data.size(), N);
    for (decltype(data.size()) i = 0; i != data.size(); ++i) {
      errs.row(static_cast<Eigen::Index>(i)) = fcn(data[i]);
    }

    CompT centered = errs.rowwise() - errs.colwise().mean();
    cov = (centered.adjoint() * centered) / (errs.rows() - 1);
  }

  return cov;
}

/**
 * Calculate sample square root information matrix I = \Sigma^{-1/2}
 * @param cov covariance
 * @param min_eig lower bound for eigenvalues of \Sigma (smaller eigenvalues are set to this value)
 * @return square root information matrix of size D x D
 */
template<typename Derived>
auto sqrt_information(const Eigen::MatrixBase<Derived> & cov, typename Derived::Scalar min_eig)
{
  Eigen::SelfAdjointEigenSolver<typename Derived::PlainMatrix> es(cov);
  return (es.eigenvectors() *
         es.eigenvalues().cwiseMax(min_eig).cwiseSqrt().cwiseInverse().asDiagonal() *
         es.eigenvectors().transpose()).eval();
}

}  // namespace cbr

#endif  // CBR_MATH__MATH_HPP_