// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2010.

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

// test_gamma_dist.cpp

// http://en.wikipedia.org/wiki/Gamma_distribution
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda366b.htm
// Also:
// Weisstein, Eric W. "Gamma Distribution."
// From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/GammaDistribution.html

#include <pch.hpp> // include directory libs/math/src/tr1/ is needed.

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/test/test_exec_monitor.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>

#include <boost/math/distributions/gamma.hpp>
    using boost::math::gamma_distribution;
#include <boost/math/tools/test.hpp>

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;
#include <limits>
  using std::numeric_limits;

template <class RealType>
RealType NaivePDF(RealType shape, RealType scale, RealType x)
{
   // Deliberately naive PDF calculator again which
   // we'll compare our pdf function.  However some
   // published values to compare against would be better....
   using namespace std;
   RealType result = log(x) * (shape - 1) - x / scale - boost::math::lgamma(shape) - log(scale) * shape;
   return exp(result);
}

template <class RealType>
void check_gamma(RealType shape, RealType scale, RealType x, RealType p, RealType q, RealType tol)
{
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         gamma_distribution<RealType>(shape, scale),    // distribution.
         x),                                            // random variable.
         p,                                             // probability.
         tol);                                          // %tolerance.
   BOOST_CHECK_CLOSE(
      ::boost::math::cdf(
         complement(
            gamma_distribution<RealType>(shape, scale), // distribution.
            x)),                                        // random variable.
         q,                                             // probability complement.
         tol);                                          // %tolerance.
   if(p < 0.999)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(
            gamma_distribution<RealType>(shape, scale),    // distribution.
            p),                                            // probability.
            x,                                             // random variable.
            tol);                                          // %tolerance.
   }
   if(q < 0.999)
   {
      BOOST_CHECK_CLOSE(
         ::boost::math::quantile(
            complement(
               gamma_distribution<RealType>(shape, scale), // distribution.
               q)),                                        // probability complement.
            x,                                             // random variable.
            tol);                                          // %tolerance.
   }
   // PDF:
   BOOST_CHECK_CLOSE(
      boost::math::pdf(
         gamma_distribution<RealType>(shape, scale),    // distribution.
         x),                                            // random variable.
         NaivePDF(shape, scale, x),                     // PDF
         tol);                                          // %tolerance.
}

template <class RealType>
void test_spots(RealType)
{
   // Basic sanity checks
   //
   // 15 decimal places expressed as a persentage.
   // The first tests use values generated by MathCAD,
   // and should be accurate to around double precision.
   //
   RealType tolerance = (std::max)(RealType(5e-14f), std::numeric_limits<RealType>::epsilon() * 20) * 100;
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;

   check_gamma(
      static_cast<RealType>(0.5),
      static_cast<RealType>(1),
      static_cast<RealType>(0.5),
      static_cast<RealType>(0.682689492137085),
      static_cast<RealType>(1-0.682689492137085),
      tolerance);
   check_gamma(
      static_cast<RealType>(2),
      static_cast<RealType>(1),
      static_cast<RealType>(0.5),
      static_cast<RealType>(0.090204010431050),
      static_cast<RealType>(1-0.090204010431050),
      tolerance);
   check_gamma(
      static_cast<RealType>(40),
      static_cast<RealType>(1),
      static_cast<RealType>(10),
      static_cast<RealType>(7.34163631456064E-13),
      static_cast<RealType>(1-7.34163631456064E-13),
      tolerance);

   //
   // Some more test data generated by the online
   // calculator at http://espse.ed.psu.edu/edpsych/faculty/rhale/hale/507Mat/statlets/free/pdist.htm
   // This has the advantage of supporting the scale parameter as well
   // as shape, but has only a few digits accuracy, and produces
   // some deeply suspect values if the shape parameter is < 1
   // (it doesn't agree with MathCAD or this implementation).
   // To be fair the incomplete gamma is tricky to get right in this area...
   //
   tolerance = 1e-5f * 100; // 5 decimal places as a persentage
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;

   check_gamma(
      static_cast<RealType>(2),
      static_cast<RealType>(1)/5,
      static_cast<RealType>(0.1),
      static_cast<RealType>(0.090204),
      static_cast<RealType>(1-0.090204),
      tolerance);
   check_gamma(
      static_cast<RealType>(2),
      static_cast<RealType>(1)/5,
      static_cast<RealType>(0.5),
      static_cast<RealType>(1-0.287298),
      static_cast<RealType>(0.287298),
      tolerance);
   check_gamma(
      static_cast<RealType>(3),
      static_cast<RealType>(2),
      static_cast<RealType>(1),
      static_cast<RealType>(0.014388),
      static_cast<RealType>(1-0.014388),
      tolerance * 10); // one less decimal place in the test value
   check_gamma(
      static_cast<RealType>(3),
      static_cast<RealType>(2),
      static_cast<RealType>(5),
      static_cast<RealType>(0.456187),
      static_cast<RealType>(1-0.456187),
      tolerance);

    RealType tol2 = boost::math::tools::epsilon<RealType>() * 5 * 100;  // 5 eps as a persentage
    gamma_distribution<RealType> dist(8, 3);
    RealType x = static_cast<RealType>(0.125);
    using namespace std; // ADL of std names.
    // mean:
    BOOST_CHECK_CLOSE(
       mean(dist)
       , static_cast<RealType>(8*3), tol2);
    // variance:
    BOOST_CHECK_CLOSE(
       variance(dist)
       , static_cast<RealType>(8*3*3), tol2);
    // std deviation:
    BOOST_CHECK_CLOSE(
       standard_deviation(dist)
       , sqrt(static_cast<RealType>(8*3*3)), tol2);
    // hazard:
    BOOST_CHECK_CLOSE(
       hazard(dist, x)
       , pdf(dist, x) / cdf(complement(dist, x)), tol2);
    // cumulative hazard:
    BOOST_CHECK_CLOSE(
       chf(dist, x)
       , -log(cdf(complement(dist, x))), tol2);
    // coefficient_of_variation:
    BOOST_CHECK_CLOSE(
       coefficient_of_variation(dist)
       , standard_deviation(dist) / mean(dist), tol2);
    // mode:
    BOOST_CHECK_CLOSE(
       mode(dist)
       , static_cast<RealType>(7 * 3), tol2);
    // skewness:
    BOOST_CHECK_CLOSE(
       skewness(dist)
       , 2 / sqrt(static_cast<RealType>(8)), tol2);
    // kertosis:
    BOOST_CHECK_CLOSE(
       kurtosis(dist)
       , 3 + 6 / static_cast<RealType>(8), tol2);
    // kertosis excess:
    BOOST_CHECK_CLOSE(
       kurtosis_excess(dist)
       , 6 / static_cast<RealType>(8), tol2);

    BOOST_CHECK_CLOSE(
       median(dist), static_cast<RealType>(23.007748327502412), // double precision test value
       (std::max)(tol2, static_cast<RealType>(std::numeric_limits<double>::epsilon() * 2 * 100))); // 2 eps as persent
    // Rely on default definition in derived accessors.

} // template <class RealType>void test_spots(RealType)

int test_main(int, char* [])
{
   // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif

   return 0;
} // int test_main(int, char* [])

/*

Output:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_gamma_dist.exe"
Running 1 test case...
Tolerance for type float is 0.000238419 %
Tolerance for type float is 0.001 %
Tolerance for type double is 5e-012 %
Tolerance for type double is 0.001 %
Tolerance for type long double is 5e-012 %
Tolerance for type long double is 0.001 %
Tolerance for type class boost::math::concepts::real_concept is 5e-012 %
Tolerance for type class boost::math::concepts::real_concept is 0.001 %
*** No errors detected

*/


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/hypot.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdHypot,Fvar) {
  using stan::math::fvar;
  using boost::math::hypot;
  using std::isnan;

  fvar<double> x(0.5,1.0);
  fvar<double> y(2.3,2.0);

  fvar<double> a = hypot(x, y);
  EXPECT_FLOAT_EQ(hypot(0.5, 2.3), a.val_);
  EXPECT_FLOAT_EQ((0.5 * 1.0 + 2.3 * 2.0) / hypot(0.5, 2.3), a.d_);

  fvar<double> z(0.0,1.0);
  fvar<double> w(-2.3,2.0);
  fvar<double> b = hypot(x, z);

  EXPECT_FLOAT_EQ(0.5, b.val_);
  EXPECT_FLOAT_EQ(1.0, b.d_);

  fvar<double> c = hypot(x, w);
  isnan(c.val_);
  isnan(c.d_);

  fvar<double> d = hypot(z, x);
  EXPECT_FLOAT_EQ(0.5, d.val_);
  EXPECT_FLOAT_EQ(1.0, d.d_);
}

TEST(AgradFwdHypot,FvarFvarDouble) {
  using stan::math::fvar;
  using boost::math::hypot;

  fvar<fvar<double> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = hypot(x,y);

  EXPECT_FLOAT_EQ(hypot(3.0,6.0), a.val_.val_);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0,6.0), a.val_.d_);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0,6.0), a.d_.val_);
  EXPECT_FLOAT_EQ(-0.059628479, a.d_.d_);
}

struct hypot_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return hypot(arg1,arg2);
  }
};

TEST(AgradFwdHypot, nan) {
  hypot_fun hypot_;
  test_nan_fwd(hypot_,3.0,5.0,false);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MATH_UNITY_ROOT_HPP
#define CRYPTO3_MATH_UNITY_ROOT_HPP

#include <type_traits>
#include <complex>

#include <boost/math/constants/constants.hpp>
#include <nil/crypto3/algebra/fields/params.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {

            template<typename FieldType>
            constexpr typename std::enable_if<std::is_same<typename FieldType::value_type, std::complex<double>>::value,
                                              typename FieldType::value_type>::type
                unity_root(const std::size_t n) {
                const double PI = boost::math::constants::pi<double>();

                return typename FieldType::value_type(cos(2 * PI / n), sin(2 * PI / n));
            }

            template<typename FieldType>
            constexpr
                typename std::enable_if<!std::is_same<typename FieldType::value_type, std::complex<double>>::value,
                                        typename FieldType::value_type>::type
                unity_root(const std::size_t n) {

                typedef typename FieldType::value_type value_type;

                const std::size_t logn = std::ceil(std::log2(n));

                if (n != (1u << logn)) {
                    throw std::invalid_argument("expected n == (1u << logn)");
                }
                if (logn > algebra::fields::arithmetic_params<FieldType>::s) {
                    throw std::invalid_argument("expected logn <= arithmetic_params<FieldType>::s");
                }

                value_type omega = value_type(algebra::fields::arithmetic_params<FieldType>::root_of_unity);
                for (std::size_t i = algebra::fields::arithmetic_params<FieldType>::s; i > logn; --i) {
                    omega *= omega;
                }

                return omega;
            }
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_MATH_UNITY_ROOT_HPP


#include "edlib/EDP/ConstructSparseMat.hpp"
#include "edlib/EDP/LocalHamiltonian.hpp"
#include "utils.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <unsupported/Eigen/KroneckerProduct>

#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>

#include <catch2/catch.hpp>

#include <algorithm>
#include <cassert>
#include <iostream>
#include <random>

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
twoQubitOp(int N, int pos1, int pos2, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v1,
           const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v2)
{
    using namespace Eigen;

    assert(pos1 < pos2); // NOLINT

    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(1, 1);
    res(0, 0) = 1.0;

    for(int i = 0; i < pos1; i++)
    {
        res = Eigen::kroneckerProduct(MatrixXd::Identity(2, 2), res).eval();
    }

    res = Eigen::kroneckerProduct(v1, res).eval();
    for(int i = pos1 + 1; i < pos2; i++)
    {
        res = Eigen::kroneckerProduct(MatrixXd::Identity(2, 2), res).eval();
    }
    res = Eigen::kroneckerProduct(v2, res).eval();
    for(int i = pos2 + 1; i < N; i++)
    {
        res = Eigen::kroneckerProduct(MatrixXd::Identity(2, 2), res).eval();
    }
    return res;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
singleQubitOp(int N, int pos, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v)
{
    using namespace Eigen;
    using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

    MatrixT res(1, 1);
    res(0, 0) = 1.0;

    for(int i = 0; i < pos; i++)
    {
        res = Eigen::kroneckerProduct(MatrixT::Identity(2, 2), res).eval();
    }

    res = Eigen::kroneckerProduct(v, res).eval();
    for(int i = pos + 1; i < N; i++)
    {
        res = Eigen::kroneckerProduct(MatrixT::Identity(2, 2), res).eval();
    }
    return res;
}

class GraphGenerator
{
private:
    int numVertices_;
    std::vector<std::pair<int, int>> allEdges_;
    std::vector<int> allVertices_;

public:
    explicit GraphGenerator(int numVertices) : numVertices_{numVertices}
    {
        for(int i = 0; i < numVertices - 1; ++i)
        {
            for(int j = i + 1; j < numVertices; ++j)
            {
                allEdges_.emplace_back(i, j);
            }
        }
        for(int i = 0; i < numVertices_; ++i)
        {
            allVertices_.emplace_back(i);
        }
    }

    template<typename RandomEngine>
    std::vector<std::pair<int, int>> createRandomGraph(RandomEngine& re, int numEdges)
    {
        std::vector<std::pair<int, int>> edges = allEdges_;
        std::shuffle(edges.begin(), edges.end(), re);
        edges.resize(numEdges);
        return edges;
    }

    template<typename RandomEngine> std::vector<int> createRandomVertexSet(RandomEngine& re, int n)
    {
        std::vector<int> v = allVertices_;
        std::shuffle(v.begin(), v.end(), re);
        v.resize(n);
        return v;
    }
};

constexpr uint32_t N = 10;
TEST_CASE("Test single qubit operators", "[LocalHamSingle]")
{
    std::random_device rd;
    std::default_random_engine re{rd()};
    std::uniform_int_distribution<> uid(0, N - 1);
    std::uniform_real_distribution<> urd;

    using cx_double = std::complex<double>;

    SECTION("Test constructing Pauli X")
    {
        edp::LocalHamiltonian<double> lh(N, 2);
        for(int n = 0; n < 100; ++n)
        {
            lh.clearTerms();
            int idx = uid(re);
            double val = urd(re);
            lh.addOneSiteTerm(idx, val * getSX());
            auto mat1 = Eigen::MatrixXd(edp::constructSparseMat<double>(1U << N, lh));
            auto mat2 = singleQubitOp<double>(N, idx, val * getSX());
            REQUIRE((mat1 - mat2).squaredNorm() < 1e-8);
        }
    }
    SECTION("Test constructing Pauli Z")
    {
        edp::LocalHamiltonian<double> lh(N, 2);
        for(int n = 0; n < 100; ++n)
        {
            lh.clearTerms();
            int idx = uid(re);
            double val = urd(re);
            lh.addOneSiteTerm(idx, val * getSZ());
            auto mat1 = Eigen::MatrixXd(edp::constructSparseMat<double>(1U << N, lh));
            auto mat2 = singleQubitOp<double>(N, idx, val * getSZ());
            REQUIRE((mat1 - mat2).squaredNorm() < 1e-8);
        }
    }
    SECTION("Test constructing Pauli Y")
    {
        edp::LocalHamiltonian<cx_double> lh(N, 2);
        for(int n = 0; n < 100; ++n)
        {
            lh.clearTerms();
            int idx = uid(re);
            double val = urd(re);
            lh.addOneSiteTerm(idx, val * getSY());
            auto mat1 = Eigen::MatrixXcd(edp::constructSparseMat<cx_double>(1U << N, lh));
            auto mat2 = singleQubitOp<cx_double>(N, idx, val * getSY());
            REQUIRE((mat1 - mat2).squaredNorm() < 1e-8);
        }
    }
}

TEST_CASE("Test 2-local Hamiltonians", "[LocalHam2loc]")
{
    std::random_device rd;
    std::mt19937_64 re{rd()};
    std::uniform_int_distribution<> uid(0, N - 1);
    std::uniform_int_distribution<> numEdgesRd(1, N * (N - 1) / 2);
    std::uniform_real_distribution<> urd;

    using cx_double = std::complex<double>;

    GraphGenerator ggen{N};

    SECTION("Random XYZ Hamiltonian")
    {
        edp::LocalHamiltonian<double> lh(N, 2);

        for(int n = 0; n < 10; ++n)
        {
            lh.clearTerms();
            int numEdges = numEdgesRd(re);
            auto edges = ggen.createRandomGraph(re, numEdges);

            auto matEx = Eigen::MatrixXd(1U << N, 1U << N);
            matEx.setZero();

            for(const auto& edge : edges)
            {
                double v1 = urd(re);
                double v2 = urd(re);
                double v3 = urd(re);
                lh.addTwoSiteTerm(edge, v1 * getSXX());
                lh.addTwoSiteTerm(edge, v2 * getSYY());
                lh.addTwoSiteTerm(edge, v3 * getSZZ());

                matEx += v1 * twoQubitOp<double>(N, edge.first, edge.second, getSX(), getSX());
                auto matYY = twoQubitOp<cx_double>(N, edge.first, edge.second, getSY(), getSY());
                matEx += v2 * matYY.real();
                matEx += v3 * twoQubitOp<double>(N, edge.first, edge.second, getSZ(), getSZ());
            }
            auto mat = Eigen::MatrixXd(edp::constructSparseMat<double>(1U << N, lh));
            REQUIRE((mat - matEx).squaredNorm() < 1e-8);
        }
    }
    SECTION("Random Field Ising Hamiltonian")
    {
        edp::LocalHamiltonian<double> lh(N, 2);

        for(int n = 0; n < 20; ++n)
        {
            lh.clearTerms();
            int numEdges = numEdgesRd(re);
            auto edges = ggen.createRandomGraph(re, numEdges);

            auto matEx = Eigen::MatrixXd(1U << N, 1U << N);
            matEx.setZero();

            for(const auto& edge : edges)
            {
                double v = urd(re);
                lh.addTwoSiteTerm(edge, v * getSZZ());

                matEx += v * twoQubitOp<double>(N, edge.first, edge.second, getSZ(), getSZ());
            }

            auto numFields = uid(re);
            auto vs = ggen.createRandomVertexSet(re, numFields);

            for(auto v : vs)
            {
                double h1 = urd(re);
                double h3 = urd(re);

                lh.addOneSiteTerm(v, h1 * getSX());
                lh.addOneSiteTerm(v, h3 * getSZ());

                matEx += h1 * singleQubitOp<double>(N, v, getSX());
                matEx += h3 * singleQubitOp<double>(N, v, getSZ());
            }

            auto mat = Eigen::MatrixXd(edp::constructSparseMat<double>(1U << N, lh));
            REQUIRE((mat - matEx).squaredNorm() < 1e-8);
        }
    }
}


// File: vector_norm.cpp

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

int main(int, char**)
{
    using namespace mtl;

    typedef std::complex<double>  cdouble;
    dense_vector<cdouble>         v(10000);

    // Initialize vector
    for (unsigned i= 0; i < size(v); i++)
	v[i]= cdouble(i+1, 10000-i);

    std::cout << "one_norm(v) is " << one_norm(v)<< "\n";

    std::cout << "two_norm(v) is " << two_norm(v)<< "\n";

    std::cout << "infinity_norm(v) is " << infinity_norm(v)<< "\n";

    // Unroll computation of two-norm to 6 independent statements
    std::cout << "two_norm<6>(v) is " << two_norm<6>(v)<< "\n";

    return 0;
}


/** @file main.cpp Demonstrates arbitrary-precision factorial and Fibonacci
  * functions using Boost.Multiprecision.
  *
  * @note This implementation uses output parameters to allow reuse of
  * previously allocated memory.  The technique also supports the potential
  * future use of custom allocators, since the caller of each function
  * allocates and configures the result object.  Moreover, the use of in-place
  * operators for assignments avoids the creation of unnecessary temporary
  * objects.
  *
  * @todo Memoize functions.  They currently require O(n) time per call.
  */

#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using big_int = boost::multiprecision::cpp_int;

using std::swap;

void factorial(big_int* r, int n)
{
    for (*r = 1; n > 1; --n) {
        *r *= n;
    }
}

void fibonacci(big_int* r, int n)
{
    *r = 1;
    for (big_int s = 1; n > 0; --n) {
        swap(*r, s);
        s += *r;
    }
}

int main()
{
    big_int r;
    for (int i = 0; i < 100; ++i) {
        factorial(&r, i);
        std::cout << i << ' ' << r << ' ';
        fibonacci(&r, i);
        std::cout << r << '\n';
    }
}


#include <iostream>
#include <iomanip>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

#include <pangolin/pangolin.h>

struct RotationMatrix
{
    Matrix3d matrix = Matrix3d::Identity();
};

ostream& operator << ( ostream& out, const RotationMatrix& r )
{
    out.setf(ios::fixed);
    Matrix3d matrix = r.matrix;
    out<<'=';
    out<<"["<<setprecision(2)<<matrix(0,0)<<","<<matrix(0,1)<<","<<matrix(0,2)<<"],"
    << "["<<matrix(1,0)<<","<<matrix(1,1)<<","<<matrix(1,2)<<"],"
    << "["<<matrix(2,0)<<","<<matrix(2,1)<<","<<matrix(2,2)<<"]";
    return out;
}

istream& operator >> (istream& in, RotationMatrix& r )
{
    return in;
}

struct TranslationVector
{
    Vector3d trans = Vector3d(0,0,0);
};

ostream& operator << (ostream& out, const TranslationVector& t)
{
    out<<"=["<<t.trans(0)<<','<<t.trans(1)<<','<<t.trans(2)<<"]";
    return out;
}

istream& operator >> ( istream& in, TranslationVector& t)
{
    return in;
}

struct QuaternionDraw
{
    Quaterniond q;
};

ostream& operator << (ostream& out, const QuaternionDraw quat )
{
    auto c = quat.q.coeffs();
    out<<"=["<<c[0]<<","<<c[1]<<","<<c[2]<<","<<c[3]<<"]";
    return out;
}

istream& operator >> (istream& in, const QuaternionDraw quat)
{
    return in;
}

int main ( int argc, char** argv )
{
    pangolin::CreateWindowAndBind ( "visualize geometry", 1000, 600 );
    glEnable ( GL_DEPTH_TEST );
    pangolin::OpenGlRenderState s_cam (
        pangolin::ProjectionMatrix ( 1000, 600, 420, 420, 500, 300, 0.1, 1000 ),
        pangolin::ModelViewLookAt ( 3,3,3,0,0,0,pangolin::AxisY )
    );

    const int UI_WIDTH = 500;

    pangolin::View& d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f/600.0f).SetHandler(new pangolin::Handler3D(s_cam));

    // ui
    pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
    pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
    pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
    pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
    pangolin::CreatePanel("ui")
        .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

    while ( !pangolin::ShouldQuit() )
    {
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

        d_cam.Activate( s_cam );

        pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
        Matrix<double,4,4> m = matrix;
        // m = m.inverse();
        RotationMatrix R;
        for (int i=0; i<3; i++)
            for (int j=0; j<3; j++)
                R.matrix(i,j) = m(j,i);
        rotation_matrix = R;

        TranslationVector t;
        t.trans = Vector3d(m(0,3), m(1,3), m(2,3));
        t.trans = -R.matrix*t.trans;
        translation_vector = t;

        TranslationVector euler;
        euler.trans = R.matrix.transpose().eulerAngles(2,1,0);
        euler_angles = euler;

        QuaternionDraw quat;
        quat.q = Quaterniond(R.matrix);
        quaternion = quat;

        glColor3f(1.0,1.0,1.0);

        pangolin::glDrawColouredCube();
        // draw the original axis
        glLineWidth(3);
        glColor3f ( 0.8f,0.f,0.f );
        glBegin ( GL_LINES );
        glVertex3f( 0,0,0 );
        glVertex3f( 10,0,0 );
        glColor3f( 0.f,0.8f,0.f);
        glVertex3f( 0,0,0 );
        glVertex3f( 0,10,0 );
        glColor3f( 0.2f,0.2f,1.f);
        glVertex3f( 0,0,0 );
        glVertex3f( 0,0,10 );
        glEnd();

        pangolin::FinishFrame();
    }
}


//  (C) Copyright Nick Thompson 2018.
//  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)

#ifndef BOOST_MATH_TOOLS_BIVARIATE_STATISTICS_HPP
#define BOOST_MATH_TOOLS_BIVARIATE_STATISTICS_HPP

#include <iterator>
#include <tuple>
#include <boost/math/tools/assert.hpp>
#include <boost/math/tools/header_deprecated.hpp>

BOOST_MATH_HEADER_DEPRECATED("<boost/math/statistics/bivariate_statistics.hpp>");

namespace boost{ namespace math{ namespace tools {

template<class Container>
auto means_and_covariance(Container const & u, Container const & v)
{
    using Real = typename Container::value_type;
    using std::size;
    BOOST_MATH_ASSERT_MSG(size(u) == size(v), "The size of each vector must be the same to compute covariance.");
    BOOST_MATH_ASSERT_MSG(size(u) > 0, "Computing covariance requires at least one sample.");

    // See Equation III.9 of "Numerically Stable, Single-Pass, Parallel Statistics Algorithms", Bennet et al.
    Real cov = 0;
    Real mu_u = u[0];
    Real mu_v = v[0];

    for(size_t i = 1; i < size(u); ++i)
    {
        Real u_tmp = (u[i] - mu_u)/(i+1);
        Real v_tmp = v[i] - mu_v;
        cov += i*u_tmp*v_tmp;
        mu_u = mu_u + u_tmp;
        mu_v = mu_v + v_tmp/(i+1);
    }

    return std::make_tuple(mu_u, mu_v, cov/size(u));
}

template<class Container>
auto covariance(Container const & u, Container const & v)
{
    auto [mu_u, mu_v, cov] = boost::math::tools::means_and_covariance(u, v);
    return cov;
}

template<class Container>
auto correlation_coefficient(Container const & u, Container const & v)
{
    using Real = typename Container::value_type;
    using std::size;
    BOOST_MATH_ASSERT_MSG(size(u) == size(v), "The size of each vector must be the same to compute covariance.");
    BOOST_MATH_ASSERT_MSG(size(u) > 0, "Computing covariance requires at least two samples.");

    Real cov = 0;
    Real mu_u = u[0];
    Real mu_v = v[0];
    Real Qu = 0;
    Real Qv = 0;

    for(size_t i = 1; i < size(u); ++i)
    {
        Real u_tmp = u[i] - mu_u;
        Real v_tmp = v[i] - mu_v;
        Qu = Qu + (i*u_tmp*u_tmp)/(i+1);
        Qv = Qv + (i*v_tmp*v_tmp)/(i+1);
        cov += i*u_tmp*v_tmp/(i+1);
        mu_u = mu_u + u_tmp/(i+1);
        mu_v = mu_v + v_tmp/(i+1);
    }

    // If both datasets are constant, then they are perfectly correlated.
    if (Qu == 0 && Qv == 0)
    {
        return Real(1);
    }
    // If one dataset is constant and the other isn't, then they have no correlation:
    if (Qu == 0 || Qv == 0)
    {
        return Real(0);
    }

    // Make sure rho in [-1, 1], even in the presence of numerical noise.
    Real rho = cov/sqrt(Qu*Qv);
    if (rho > 1) {
        rho = 1;
    }
    if (rho < -1) {
        rho = -1;
    }
    return rho;
}

}}}
#endif


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

namespace dr {

/// Interpolate linearly between two vectors.
/**
 * At factor 0, the first vector is returned, at factor 1 the second.
 */
template<typename Derived1, typename Derived2>
auto interpolateVector(
	Eigen::MatrixBase<Derived1> const & a, ///< The first vector.
	Eigen::MatrixBase<Derived2> const & b, ///< The second vector.
	double factor                          ///< The interpolation factor.
) -> decltype(a + factor * (b - a)) {
	return a + factor * (b - a);
}

/// Interpolate spherical linearly between two rotations.
/**
 * At factor 0, the first rotation is returned, at factor 1 the second.
 */
template<typename T>
Eigen::AngleAxis<T> interpolateRotation(
	Eigen::AngleAxis<T> const & a, ///< The first rotation.
	Eigen::AngleAxis<T> const & b, ///< The second rotation.
	double factor                  ///< The interpolation factor.
) {
	Eigen::AngleAxis<T> difference = b * a.inverse();
	difference.angle() *= factor;
	return difference * a;
}

/// Interpolate spherical linearly between two rotations.
/**
 * At factor 0, the first rotation is returned, at factor 1 the second.
 */
template<typename T>
Eigen::Quaternion<T> interpolateRotation(
	Eigen::Quaternion<T> const & a, ///< The first rotation.
	Eigen::Quaternion<T> const & b, ///< The second rotation.
	double factor                   ///< The interpolation factor.
) {
	return a.slerp(factor, b);
}

/// Interpolate spherical linearly between two rotations.
/**
 * At factor 0, the first rotation is returned, at factor 1 the second.
 */
template<typename Derived1, typename Derived2>
Eigen::Quaternion<typename Eigen::MatrixBase<Derived1>::Scalar> interpolateRotation(
	Eigen::MatrixBase<Derived1> const & a, ///< The first rotation.
	Eigen::MatrixBase<Derived2> const & b, ///< The second rotation.
	double factor                          ///< The interpolation factor.
) {
	using Scalar1 = typename Eigen::MatrixBase<Derived1>::Scalar;
	using Scalar2 = typename Eigen::MatrixBase<Derived2>::Scalar;
	return interpolateRotation<Scalar1>(Eigen::Quaternion<Scalar1>(a), Eigen::Quaternion<Scalar2>(b), factor);
}

/// Interpolate spherical linearly between two isometries.
/**
 * At factor 0, the first isometry is returned, at factor 1 the second.
 * The translation will be interpolated linearly and the rotation spherial linearly.
 */
Eigen::Isometry3d interpolateIsometry(
	Eigen::Isometry3d const & a, ///< The first isometry.
	Eigen::Isometry3d const & b, ///< The second isometry.
	double factor                ///< The interpolation factor.
) {
	return Eigen::Translation3d{interpolateVector(a.translation(), b.translation(), factor)}
		* interpolateRotation(Eigen::Quaterniond{a.rotation()}, Eigen::Quaterniond{b.rotation()}, factor);
}

}


/*!
 * Created by leanne on 3/20/21.
 */
#include "HydraulicLib/ConvEigen.h"
#include <Eigen/QR>
#include <fstream>
#include <iostream>
#include <sstream> // std::stringstream
#include <stdexcept> // std::runtime_error
#include <utility> // std::pair
#include <string>
#include <vector>
//#include "include/read_write.h"
//#include "include/conv.h"
#include "HydraulicLib/NetworkSolve.h"
using Eigen::VectorXd;
using std::cerr;
using std::cout;

MatrixXd calculateSoln() {
    MatrixXd data;

    // load the matrix from the file
    // data = openData("/home/leanne/CLionProjects/SimpleHydraulicNetwork/inputs/matrix.csv");
    data = openData("inputs/matrix.csv");
    VectorXd Demand_A = data.col(0);
    VectorXd Demand_B = data.col(1);
    VectorXd Demand_C = data.col(2);
    VectorXd Demand_D = data.col(3);
    VectorXd Demand_E = data.col(4);
    VectorXd Demand_F = data.col(5);
    VectorXd Demand_z0(8760);
    Demand_z0.setZero();
    VectorXd Demand_z4(8760);
    Demand_z4.setZero();

    Demand_A = Demand_A/4.2/15;
    Demand_B = Demand_B/4.2/15;
    Demand_C = Demand_C/4.2/15;
    Demand_D = Demand_D/4.2/15;
    Demand_E = Demand_E/4.2/15;
    Demand_F = Demand_F/4.2/15;

    //   cerr << Demand_C.size() << "\n";
    MatrixXd B(8760, 8);
    B << Demand_z0, Demand_C, Demand_B, Demand_D, Demand_z4, Demand_A, Demand_E, Demand_F;
    //   cerr << B << "\n";

    MatrixXd Q = MatrixXd::Zero(8760, 8);
    Q.col(1) = Demand_C;
    Q.col(2) = Demand_B;
    Q.col( 3) = Demand_D;
    Q.col(4) = Demand_A + Demand_E + Demand_F;
    Q.col(5) = Demand_A;
    Q.col(6) = Demand_E;
    Q.col(7) = Demand_F;
    Q.col(0) = Demand_C + Demand_B + Demand_D + Demand_A + Demand_E + Demand_F;
    return Q;
}
void NetworkSolve() {
    auto result = calculateSoln();
    saveData("outputs/flowQ.csv", result);
}

/*
 * Our accuracy are good but we are getting some systematic error as the solutions from Matlab and C++ deviate.
 * This might due to the non-empty nullspace. If our matrix is rank deficient the solution may have an arbitrary large component added in the null space.
 * In other words if Ax = b then the computed solution might be x' = x* + x0, where A*x0 = 0, so r = A*x'-b = 0.
 * So, we could have perfect accuracy but still produce different solutions due to the non-empty nullspace.
 * If my system is well-conditioned, I will not need to worry about that.  If it isn't, it might help explain the systematic error I'm getting.
 *
 */

/*
 * Precision could be lost when reading/writing CSV file
 * Residual error (or existence of solutions)
 * Rank deficiency
 * Well conditionness
 */


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-exponential
    Function object implementing log10 capabilities

    base ten logarithm function. For integer input types log10 return the truncation
    of the real result.

    @par Semantic:

    For every parameter of floating type T

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

    is similar to:

    @code
    T r = log(x)/log(10);
    @endcode

    - log10(x) return Nan for negative enties (peculiarly Mzero
    for floating numbers).

    @par Decorators

    std_ for floating entries

    @see log, log2, log1p, is_negative, Mzero

  **/
  Value log10(Value const& x);
} }
#endif

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

#endif


#include "OpenICV/Core/icvFunction.h"
#include "OpenICV/Core/icvFunctionFactory.h"
#include "OpenICV/Core/icvSubscriber.h"
#include "OpenICV/Core/icvPublisher.h"
#include "OpenICV/Basis/icvPrimitiveData.hxx"

#include <boost/thread/thread.hpp>

using namespace icv;
using namespace icv::core;

class PolynomialFunction : public icvFunction
{
public:
    PolynomialFunction(icv_shared_ptr<const icvMetaData> info) : icvFunction(1, 1, info)
    {
        if (_information.Contains("coefficients"))
        {
            auto arr = _information.GetArray("coefficients");
            for (int i = 0; i < arr.Size(); i++)
                _coeffs.push_back(arr.GetDecimal(i));
            _information.Remove("coefficients");
        }
    }
    PolynomialFunction() : PolynomialFunction(ICV_NULLPTR) {}

    virtual void Execute(icvDataObject** inData, icvDataObject** outData) ICV_OVERRIDE
    {
        double data = *static_cast<icvDoubleData*>(inData[0]);
        double result = data;
        for (double coeff : _coeffs) result = result * coeff + data;
        outData[0]->As<icvDoubleData>() = result;

        ICV_LOG_TRACE << "Filter Data from " << data << " to " << result;
    }
private:
    std::vector<double> _coeffs;
};

ICV_REGISTER_FUNCTION(PolynomialFunction)


//
// This file is part of the Oxford RSE CMake Course
// (https://github.com/OxfordRSE/CMakeCourse) which is released under the MIT
// license. See accompanying LICENSE for copyright notice and full details.
//

#include "MyLibrary.hpp"

#include "Exception.hpp"

#include <boost/math/special_functions/prime.hpp>

namespace cpp_template {

int get_nth_prime(int n) {
  namespace bm = boost::math;
  namespace b = boost;

  if (n < 0) {
    throw Exception("non-negative argument required");
  }

  if (static_cast<b::uint32_t>(n) > bm::max_prime) {
    throw Exception("argument less than " + std::to_string(bm::max_prime) +
                    " required");
  }

  return static_cast<int>(boost::math::prime(static_cast<unsigned>(n)));
}

} // namespace cpp_template


#include "Constraint.h"
#include "AttachmentConstraint.h"
#include <Eigen/Dense>
#include <iostream>
using namespace FEM;
AttachmentConstraint::
AttachmentConstraint(const double& stiffness,int i0,const Eigen::Vector2d& p)
	:Constraint(stiffness),mi0(i0),mp(p)
{

}

double
AttachmentConstraint::
EvalPotentialEnergy(const Eigen::VectorXd& x)
{
	Eigen::Vector2d x_p0 = x.block<2,1>(mi0*2,0) - mp;

    return 0.5*mStiffness*x_p0.squaredNorm();
}
void
AttachmentConstraint::
EvalGradient(const Eigen::VectorXd& x, Eigen::VectorXd& gradient)
{
	Eigen::Vector2d x_p0 = x.block<2,1>(mi0*2,0) - mp;
	gradient.block<2,1>(mi0*2,0) += mStiffness*x_p0;
}

void
AttachmentConstraint::
EvalHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& dx, Eigen::VectorXd& dg)
{
	//Compute H*x
	dg.block<2,1>(mi0*2,0) += mStiffness*dx.block<2,1>(mi0*2,0);
}

void
AttachmentConstraint::
EvaluateDVector(int index, const Eigen::VectorXd& x,Eigen::VectorXd& d)
{
	d.block<2,1>(2*index,0) = mp;
}
void
AttachmentConstraint::
EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
{
	J_triplets.push_back(Eigen::Triplet<double>(2*mi0, 2*index, mStiffness));
	J_triplets.push_back(Eigen::Triplet<double>(2*mi0+1, 2*index+1, mStiffness));
}
void
AttachmentConstraint::
EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
{
	L_triplets.push_back(Eigen::Triplet<double>(2*mi0+0, 2*mi0+0, mStiffness));
	L_triplets.push_back(Eigen::Triplet<double>(2*mi0+1, 2*mi0+1, mStiffness));
}

int
AttachmentConstraint::
GetDof()
{
	return 1;
}
int
AttachmentConstraint::
GetNumHessianTriplets()
{
	return 2;
}

ConstraintType
AttachmentConstraint::
GetType()
{
	return ConstraintType::ATTACHMENT;
}
void
AttachmentConstraint::
AddOffset(const int& offset)
{
	mi0+=offset;
}

Eigen::Vector2d&
AttachmentConstraint::
GetP()
{
	return mp;
}
int&
AttachmentConstraint::
GetI0()
{
	return mi0;
}


// =========================================================================
// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
// =========================================================================

#include <iostream>

#include <boost/program_options.hpp>

#include <PUJ_ML/Helpers/CSV.h>
#include <PUJ_ML/Model/Linear.h>
#include <PUJ_ML/Optimizer/GradientDescent.h>

// -- Types
using TScalar = double;
using TModel = PUJ_ML::Model::Linear< TScalar >;
using TMatrix = TModel::TMatrix;
namespace po = boost::program_options;

// -- Main --
int main( int argc, char** argv )
{
  std::string csv = "[NO INPUT FILE]";
  TScalar alpha = 1e-2;
  TScalar lambda = 0;
  TScalar epsilon = std::numeric_limits< TScalar >::epsilon( );
  unsigned long long epochs = 10000;
  unsigned long long debug_step = 100;
  bool use_LASSO = false;

  po::options_description desc( "Allowed parameters" );
  desc.add_options( )
    ( "help,h", "Help message" )
    ( "alpha,a", po::value( &alpha )->default_value( alpha ), "Learning rate" )
    ( "lambda,l", po::value( &lambda )->default_value( lambda ), "Regularization" )
    ( "LASSO", po::bool_switch( &use_LASSO )->default_value( use_LASSO ), "Use LASSO?" )
    ( "epsilon,e", po::value( &epsilon )->default_value( epsilon ), "Epsilon" )
    ( "epochs", po::value( &epochs )->default_value( epochs ), "Epochs" )
    ( "debug_step", po::value( &debug_step )->default_value( debug_step ), "Debug step" )
    ( "csv", po::value( &csv )->default_value( csv ), "Input file" )
    ;

  po::variables_map vm;
  po::store( po::parse_command_line( argc, argv, desc ), vm );
  po::notify( vm );
  if( vm.count( "help" ) )
  {
    std::cerr << desc << std::endl;
    return( EXIT_FAILURE );
  } // end if

  auto D = PUJ_ML::Helpers::CSV::Read< TMatrix >( csv, true, "," );
  unsigned long long p = 1;
  unsigned long long m = D.rows( );
  unsigned long long n = D.cols( ) - p;
  TMatrix X = D.block( 0, 0, m, n );
  TMatrix Y = D.block( 0, n, m, p );

  TModel model;
  model.SetParameters( TModel::TCol::Zero( n + 1 ) );
  TModel::Cost cost( &model, X, Y );

  PUJ_ML::Optimizer::GradientDescent< TModel > opt;
  opt.SetCost( cost );
  opt.SetLearningRate( alpha );
  opt.SetRegularizationCoefficient( lambda );
  if( use_LASSO ) opt.SetRegularizationToLASSO( );
  else            opt.SetRegularizationToRidge( );
  opt.SetEpsilon( epsilon );
  opt.SetNumberOfEpochs( epochs );
  opt.SetDebugStep( debug_step );

  TScalar final_cost;
  unsigned long long final_epochs;
  opt.SetDebug(
    [&]( unsigned long long i, TScalar J, bool show ) -> bool
    {
      if( show )
        std::cout << i << " " << J << std::endl;
      final_cost = J;
      final_epochs = i;
      return( false );
    }
    );
  opt.Fit( );

  std::cout << "---------------------" << std::endl;
  std::cout << "Fitted model : " << model << std::endl;
  std::cout << "Final cost   : " << final_cost << std::endl;
  std::cout << "Final epochs : " << final_epochs << std::endl;
  std::cout << "---------------------" << std::endl;

  /* TODO
     std::cout << X.colwise( ).minCoeff( ) << std::endl;
     std::cout << X.colwise( ).maxCoeff( ) << std::endl;
     TModel::TCol L = TModel::TCol::LinSpaced( 100, -10, 10 );
  */

  return( EXIT_SUCCESS );
}

// eof - $RCSfile$


/**
 * @file radauthreetimestepping.cc
 * @brief NPDE homework RadauThreeTimestepping
 * @author Erick Schulz, edited by Oliver Rietmann
 * @date 08/04/2019
 * @copyright Developed at ETH Zurich
 */

#include "radauthreetimestepping.h"

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

#include <Eigen/Core>
#include <Eigen/SparseLU>
#include <cmath>
#include <iostream>
#include <unsupported/Eigen/KroneckerProduct>

namespace RadauThreeTimestepping {

/**
 * @brief Implementation of the right hand side (time dependent) source vector
 * for the parabolic heat equation
 * @param dofh A reference to the DOFHandler
 * @param time The time at which to evaluate the source vector
 * @returns The source vector at time `time`
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd rhsVectorheatSource(const lf::assemble::DofHandler &dofh,
                                    double time) {
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  // Right-hand side vector has to be set to zero initially
  Eigen::VectorXd phi(N_dofs);
#if SOLUTION
  // Functor for computing the source function at 2d coordinates
  auto f = [time](Eigen::Vector2d x) -> double {
    const double PI = 3.14159265358979323846;
    Eigen::Vector2d v(std::cos(time * PI), std::sin(time * PI));
    return ((x - 0.5 * v).norm() < 0.5) ? 1.0 : 0.0;
  };
  auto mesh_p = dofh.Mesh();  // pointer to current mesh
  phi.setZero();

  /* Assembling right-hand side source vector */
  // Initialize object taking care of local computations on all cells.
  TrapRuleLinFEElemVecProvider<decltype(f)> elvec_builder(f);
  // Computing right hand side vector
  // Invoke assembly on cells (codim == 0 as first agrument)
  lf::assemble::AssembleVectorLocally(0, dofh, elvec_builder, phi);

  /* Enforce the zero Dirichlet boundary conditions */
  // Obtain an array of boolean flags for the vertices of the mesh: 'true'
  // indicates that the vertex lies on the boundary. This predicate will
  // guarantee that the computations are carried only on the boundary vertices
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2)};
  // Creating predicate that will guarantee that the computations are carried
  // only on the vertices of the mesh using the boundary flags
  auto vertices_predicate =
      [&bd_flags](const lf::mesh::Entity &vertex) -> bool {
    return bd_flags(vertex);
  };
  // Assigning zero to the boundary values of phi
  for (const lf::mesh::Entity *vertex : mesh_p->Entities(2)) {
    if (bd_flags(*vertex)) {
      auto dof_idx = dofh.GlobalDofIndices(*vertex);
      LF_ASSERT_MSG(
          dofh.NumLocalDofs(*vertex) == 1,
          "Too many global indices were returned for a vertex entity!");
      phi(dof_idx[0]) = 0.0;
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return phi;
}
/* SAM_LISTING_END_1 */

/**
 * @brief Heat evolution solver: the solver obtains the
 * discrete evolution operator from the Radau3MOLTimestepper class and
 * repeatedly iterates its applicaiton starting from the initial condition
 * @param dofh The DOFHandler object
 * @param m is total number of steps until final time final_time (double)
 * @param final_time The duration for which to solve the PDE
 * @returns The solution at the final timestep
 */
/* SAM_LISTING_BEGIN_6 */
Eigen::VectorXd solveHeatEvolution(const lf::assemble::DofHandler &dofh,
                                   unsigned int m, double final_time) {
  Eigen::VectorXd discrete_heat_sol(dofh.NumDofs());
#if SOLUTION
  double tau = final_time / m;                          // step size
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());  // dim. of FE space

  std::cout << "*********************************************************"
            << std::endl;
  std::cout << "\n>>> SolveHeatEvolution: m = " << m << ", N = " << N_dofs
            << std::endl;
  /* Setting up the problem information */
  // Precomputing the required data for the Runge-Kutta method
  // Assemble the Runge-Kutta Radau IIA 2-stages method solver (order 3)
  Radau3MOLTimestepper radau_solver(dofh);
  // Starting with the zero initial condition vector
  Eigen::VectorXd discrete_solution_cur =
      radau_solver.discreteEvolutionOperator(0.0, tau,
                                             Eigen::VectorXd::Zero(N_dofs));

  std::cout << "\n>> Iterating the action of discreteEvolutionOperator"
            << std::endl;
  /* Evolving the parabolic heat system */
  // While less elegant, we use a current and next step solution vector in the
  // iteration to stay away from potential harming aliasing effects of putting
  // an Eigen::Vector on both sides of an assignment statement.
  Eigen::VectorXd discrete_solution_next;
  for (int i = 1; i < m; i++) {
    discrete_solution_next = radau_solver.discreteEvolutionOperator(
        i * tau, tau, discrete_solution_cur);
    discrete_solution_cur = discrete_solution_next;
  }
  discrete_heat_sol = discrete_solution_cur;
#else
  //====================
  // Your code goes here
  //====================
#endif
  return discrete_heat_sol;
}
/* SAM_LISTING_END_6 */

/* Implementing member function Eval of class LinFEMassMatrixProvider*/
Eigen::Matrix<double, 3, 3> LinFEMassMatrixProvider::Eval(
    const lf::mesh::Entity &tria) {
  Eigen::Matrix<double, 3, 3> elMat;
#if SOLUTION
  // Throw error in case no triangular cell
  LF_VERIFY_MSG(tria.RefEl() == lf::base::RefEl::kTria(),
                "Unsupported cell type " << tria.RefEl());
  // Compute the area of the triangle cell
  const double area = lf::geometry::Volume(*(tria.Geometry()));
  // Assemble the mass element matrix over the cell
  // clang-format off
  elMat << 2.0, 1.0, 1.0,
           1.0, 2.0, 1.0,
           1.0, 1.0, 2.0;
  // clang-format on
  elMat *= area / 12.0;
#else
  //====================
  // Your code goes here
  //====================
#endif
  return elMat;  // return the local mass element matrix
}

/* Implementing constructor of class Radau3MOLTimestepper */
/* SAM_LISTING_BEGIN_4 */
Radau3MOLTimestepper::Radau3MOLTimestepper(const lf::assemble::DofHandler &dofh)
    : dofh_(dofh) {
#if SOLUTION
  std::cout << "\n>> Constructing SRadau3MOLTimestepper " << std::endl;
  auto mesh_p = dofh.Mesh();  // pointer to current mesh

  // Instantiating Galerkin matrices to be pre-computed
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  // Matrices in triplet format holding Galerkin matrices, zero initially.
  lf::assemble::COOMatrix<double> A_COO(N_dofs,
                                        N_dofs);  // element matrix Laplace
  lf::assemble::COOMatrix<double> M_COO(N_dofs,
                                        N_dofs);  // element mass matrix

  std::cout << "> Initializing the Galerking local matrices builders"
            << std::endl;
  // Initialize classes containing the information required for the
  // local computations of the Galerkin matrices. Simple implementations of
  // LinFEMassMatrixProvider and TrapRuleLinFEElemVecProvider adapted to this
  // particular problem was written to spare some of the overhead calculations
  // involved in the use of the more general LehrFEM++ matrices providers.
  lf::uscalfe::LinearFELaplaceElementMatrix elLapMat_builder;
  LinFEMassMatrixProvider elMassMat_builder;

  std::cout << "> Assembling Galerking matrices in COO format" << std::endl;
  // Compute the Galerkin matrices
  // Invoke assembly on cells (co-dimension = 0 as first argument)
  // Information about the mesh and the local-to-global map is passed through
  // a Dofhandler object, argument 'dofh'. This function call adds triplets to
  // the internal COO-format representation of the sparse matrices A and M.
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elLapMat_builder, A_COO);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elMassMat_builder, M_COO);

  // Enforcing zero Dirichlet boundary conditions
  // Obtain an array of boolean flags for the vertices of the mesh: 'true'
  // indicates that the vertex lies on the boundary.
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 2)};
  // Index predicate for the selectvals FUNCTOR of dropMatrixRowsColumns
  auto bdy_vertices_selector = [&bd_flags, &dofh](unsigned int idx) -> bool {
    return bd_flags(dofh.Entity(idx));
  };
  dropMatrixRowsColumns(bdy_vertices_selector, A_COO);
  dropMatrixRowsColumns(bdy_vertices_selector, M_COO);

  std::cout << "> Converting triplets to sparse matrices" << std::endl;
  // Creating the private Galerkin stiffness and mass matrices
  A_ = A_COO.makeSparse();
  Eigen::SparseMatrix<double> M = M_COO.makeSparse();

  // Runge-Kutta matrices defining the 2-stage Radau timestepping. In the
  // Butcher tableau, this corresponds to c = (1/3 1)^T (top-left column
  // vector), b^T = (3/4 1/4) (bottom-right row vector), U_11 = 5/12, U_12 =
  // -1/12, U_21 = 3/4, U_22 = 1/4 (top-right block); values are fixed in
  // time
  // clang-format off
    U_ << 5.0/12.0, -1.0/12.0,
              0.75,      0.25;
    c_ << 1.0/3.0, 1.0;
    b_ << 0.75, 0.25;
  // clang-format on
  // Precomputing the kronecker products involved in the implicit linear system
  // for the increments of the RADAU-2 method
  M_Kp_ = Eigen::kroneckerProduct(Eigen::Matrix<double, 2, 2>::Identity(), M);
  A_Kp_ = Eigen::kroneckerProduct(U_, A_);
#else
  //====================
  // Your code goes here
  // Add any additional members you need in the header file
  //====================
#endif
}
/* SAM_LISTING_END_4 */

/* Implementation of Radau3MOLTimestepper member functions */
// The function discreteEvolutionOperator() returns the discretized evolution
// operator as obtained from the Runge-Kutta Radau IIA 2-stages method using the
// Butcher table as stored in the Radau3MOLTimestepper class
/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd Radau3MOLTimestepper::discreteEvolutionOperator(
    double time, double tau, const Eigen::VectorXd &mu) const {
  Eigen::VectorXd discrete_evolution_operator(dofh_.NumDofs());
#if SOLUTION
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh_.NumDofs());
  LF_VERIFY_MSG(N_dofs == mu.size(),
                "Dimension mismatch between the number of degrees of freedom "
                "and the dimension of the argument vector.");

  // Building the linear system for the implicitely defined increments
  // Assembling the right hand side using block initialization
  Eigen::VectorXd linSys_rhs(2 * N_dofs);
  Eigen::VectorXd rhs_subtraction_term = A_ * mu;  // precomputation
  linSys_rhs << rhsVectorheatSource(dofh_, time + c_[0] * tau) -
                    rhs_subtraction_term,
      rhsVectorheatSource(dofh_, time + tau) - rhs_subtraction_term;

  // Implicit Runge-Kutta methods lead to systems of equations that must be
  // solved in order to obtained the increments.
  Eigen::SparseMatrix<double> linSys_mat;
  // Assembling the system right hand side matrix using the (unfortunately
  // officially not supported) Eigen Kronecker product
  linSys_mat = M_Kp_ + tau * A_Kp_;
  LF_VERIFY_MSG(linSys_mat.rows() == linSys_mat.cols(),
                "The linSys_mat Eigen matrix is not squared.");
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(linSys_mat);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "LU decomposition failed");

  // Solve linear system using Eigen's sparse direct elimination
  Eigen::VectorXd k_vec = solver.solve(linSys_rhs);
  LF_VERIFY_MSG(solver.info() == Eigen::Success, "Solving LSE failed");

  // Compute action of the discrete evolution operator on argument vec
  discrete_evolution_operator = mu + tau * (b_[0] * k_vec.topRows(N_dofs) +
                                            b_[1] * k_vec.bottomRows(N_dofs));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return discrete_evolution_operator;
}
/* SAM_LISTING_END_5 */

}  // namespace RadauThreeTimestepping


/*!
 * @file     multivariate_normal_eigen_test.cpp
 * @author   Giuseppe Rizzi
 * @date     21.07.2020
 * @version  1.0
 * @brief    description
 */

#include "mppi/sampler/multivariate_normal_eigen.h"
#include <gtest/gtest.h>
#include <math.h>
#include <Eigen/Dense>
#include <array>
#include <chrono>

TEST(MultivaritateNormal, Sample1D) {
  Eigen::MatrixXd covar(1, 1);
  Eigen::VectorXd mean(1);
  covar << 10.0;
  mean << 3.0;
  auto mn = mppi::multivariate_normal(mean, covar);
  ASSERT_TRUE(mn().size() == 1);

  std::map<int, int> hist{};
  for (size_t i = 0; i < 10000; i++) ++hist[std::round(mn()(0))];

  std::cout << "Mean=" << mean(0) << ", covar=" << covar(0, 0) << std::endl;
  for (auto p : hist) {
    std::cout << std::setw(2) << p.first << ' '
              << std::string(p.second / 10, '*') << '\n';
  }
}

TEST(MultivaritateNormal, Sample2D) {
  Eigen::MatrixXd covar(2, 2);
  Eigen::VectorXd mean(2);
  covar << 20.0, 0.0, 0.0, 5.0;
  mean << 3.0, -1.0;
  auto mn = mppi::multivariate_normal(mean, covar);
  ASSERT_TRUE(mn().size() == 2);

  std::map<int, int> hist1{};
  std::map<int, int> hist2{};

  for (size_t i = 0; i < 10000; i++) {
    ++hist1[std::round(mn()(0))];
    ++hist2[std::round(mn()(1))];
  }
  std::cout << std::endl;

  std::cout << "First variable: mean=" << mean(0) << ", covar=" << covar(0, 0)
            << std::endl;
  for (auto p : hist1) {
    std::cout << std::setw(2) << p.first << ' '
              << std::string(p.second / 10, '*') << '\n';
  }

  std::cout << "Second variable: mean=" << mean(1) << ", covar=" << covar(1, 1)
            << std::endl;
  for (auto p : hist2) {
    std::cout << std::setw(2) << p.first << ' '
              << std::string(p.second / 10, '*') << '\n';
  }
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


//
// Copyright (c) 2018 CNRS
//

#include "pinocchio/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/explog.hpp"

#include <iostream>

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,0>
computeV(const Eigen::MatrixBase<Vector3Like> & v3)
{
  typedef typename Vector3Like::Scalar Scalar;
  typedef Eigen::Matrix<Scalar,3,3,0> ReturnType;
  typedef ReturnType Matrix3;

  Scalar t2 = v3.squaredNorm();
  const Scalar t = pinocchio::math::sqrt(t2);
  Scalar alpha, beta, zeta;

  if (t < 1e-4)
  {
    alpha = Scalar(1) + t2/Scalar(6) - t2*t2/Scalar(120);
    beta = Scalar(1)/Scalar(2) - t2/Scalar(24);
    zeta = Scalar(1)/Scalar(6) - t2/Scalar(120);
  }
  else
  {
    Scalar st,ct; pinocchio::SINCOS(t,&st,&ct);
    alpha = st/t;
    beta = (1-ct)/t2;
    zeta = (1 - alpha)/(t2);
  }

  Matrix3 V
  = alpha * Matrix3::Identity()
  + beta * pinocchio::skew(v3)
  + zeta * v3 * v3.transpose();

  return V;
}

template<typename Vector3Like>
Eigen::Matrix<typename Vector3Like::Scalar,3,3,0>
computeVinv(const Eigen::MatrixBase<Vector3Like> & v3)
{
  typedef typename Vector3Like::Scalar Scalar;
  typedef Eigen::Matrix<Scalar,3,3,0> ReturnType;
  typedef ReturnType Matrix3;

  Scalar t2 = v3.squaredNorm();
  const Scalar t = pinocchio::math::sqrt(t2);

  Scalar alpha, beta;
  if (t < 1e-4)
  {
    alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
    beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
  }
  else
  {
    Scalar st,ct; pinocchio::SINCOS(t,&st,&ct);
    alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
    beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
  }

  Matrix3 Vinv
  = alpha * Matrix3::Identity()
  - 0.5 * pinocchio::skew(v3)
  + beta * v3 * v3.transpose();

  return Vinv;
}

BOOST_AUTO_TEST_CASE(test_log3)
{
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef double Scalar;
  typedef AD<Scalar> ADScalar;

  typedef pinocchio::SE3Tpl<Scalar> SE3;
  typedef pinocchio::MotionTpl<Scalar> Motion;
  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
  typedef pinocchio::MotionTpl<ADScalar> ADMotion;

  Motion v(Motion::Zero());
  SE3 M(SE3::Random()); M.translation().setZero();

  SE3::Matrix3 rot_next = M.rotation() * pinocchio::exp3(v.angular());

  SE3::Matrix3 Jlog3;
  pinocchio::Jlog3(M.rotation(), Jlog3);

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

  ADMotion ad_v(v.cast<ADScalar>());
  ADSE3 ad_M(M.cast<ADScalar>());
  ADSE3::Matrix3 rot = ad_M.rotation();

  ADVector X(3);

  X = ad_v.angular();

  CppAD::Independent(X);
  ADMotion::Vector3 X_ = X;

  ADSE3::Matrix3 ad_rot_next = rot * pinocchio::exp3(X_);
  ADMotion::Vector3 log_R_next = pinocchio::log3(ad_rot_next);

  ADVector Y(3);
  Y = log_R_next;

  CppAD::ADFun<Scalar> map(X,Y);

  CPPAD_TESTVECTOR(Scalar) x(3);
  Eigen::Map<Motion::Vector3>(x.data()).setZero();

  CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0,x);
  Motion::Vector3 nu_next(Eigen::Map<Motion::Vector3>(nu_next_vec.data()));

  SE3::Matrix3 rot_next_from_map = pinocchio::exp3(nu_next);

  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);

  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),3,3);

  BOOST_CHECK(rot_next_from_map.isApprox(rot_next));
  BOOST_CHECK(jacobian.isApprox(Jlog3));
}

BOOST_AUTO_TEST_CASE(test_explog_translation)
{
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef double Scalar;
  typedef AD<Scalar> ADScalar;

  typedef pinocchio::SE3Tpl<Scalar> SE3;
  typedef pinocchio::MotionTpl<Scalar> Motion;
  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
  typedef pinocchio::MotionTpl<ADScalar> ADMotion;

  Motion v(Motion::Zero());
  SE3 M(SE3::Random()); //M.rotation().setIdentity();

  {
    Motion::Vector3 v3_test; v3_test.setRandom();
    SE3::Matrix3 V = computeV(v3_test);
    SE3::Matrix3 Vinv = computeVinv(v3_test);

    BOOST_CHECK((V*Vinv).isIdentity());
  }

  SE3 M_next = M * pinocchio::exp6(v);
//  BOOST_CHECK(M_next.rotation().isIdentity());

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

  ADMotion ad_v(v.cast<ADScalar>());
  ADSE3 ad_M(M.cast<ADScalar>());

  ADVector X(6);

  X = ad_v.toVector();

  CppAD::Independent(X);
  ADMotion::Vector6 X_ = X;

  pinocchio::MotionRef<ADMotion::Vector6> ad_v_ref(X_);
  ADSE3 ad_M_next = ad_M * pinocchio::exp6(ad_v_ref);

  ADVector Y(6);
  Y.head<3>() = ad_M_next.translation();
  Y.tail<3>() = ad_M.translation() + ad_M.rotation() * computeV(ad_v_ref.angular()) * ad_v_ref.linear();

  CppAD::ADFun<Scalar> map(X,Y);

  CPPAD_TESTVECTOR(Scalar) x((size_t)X.size());
  Eigen::Map<Motion::Vector6>(x.data()).setZero();

  CPPAD_TESTVECTOR(Scalar) translation_vec = map.Forward(0,x);
  Motion::Vector3 translation1(Eigen::Map<Motion::Vector3>(translation_vec.data()));
  Motion::Vector3 translation2(Eigen::Map<Motion::Vector3>(translation_vec.data()+3));
  BOOST_CHECK(translation1.isApprox(M_next.translation()));
  BOOST_CHECK(translation2.isApprox(M_next.translation()));

  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);

  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),Y.size(),X.size());

  BOOST_CHECK(jacobian.topLeftCorner(3,3).isApprox(M.rotation()));

}

BOOST_AUTO_TEST_CASE(test_explog)
{
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef double Scalar;
  typedef AD<Scalar> ADScalar;

  typedef pinocchio::SE3Tpl<Scalar> SE3;
  typedef pinocchio::MotionTpl<Scalar> Motion;
  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
  typedef pinocchio::MotionTpl<ADScalar> ADMotion;

  Motion v(Motion::Zero());
  SE3 M(SE3::Random()); //M.translation().setZero();

  SE3::Matrix6 Jlog6;
  pinocchio::Jlog6(M, Jlog6);

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

  ADMotion ad_v(v.cast<ADScalar>());
  ADSE3 ad_M(M.cast<ADScalar>());

  ADVector X(6);

  X.segment<3>(Motion::LINEAR) = ad_v.linear();
  X.segment<3>(Motion::ANGULAR) = ad_v.angular();

  CppAD::Independent(X);
  ADMotion::Vector6 X_ = X;
  pinocchio::MotionRef< ADMotion::Vector6> v_X(X_);

  ADSE3 ad_M_next = ad_M * pinocchio::exp6(v_X);
  ADMotion ad_log_M_next = pinocchio::log6(ad_M_next);

  ADVector Y(6);
  Y.segment<3>(Motion::LINEAR) = ad_log_M_next.linear();
  Y.segment<3>(Motion::ANGULAR) = ad_log_M_next.angular();

  CppAD::ADFun<Scalar> map(X,Y);

  CPPAD_TESTVECTOR(Scalar) x(6);
  Eigen::Map<Motion::Vector6>(x.data()).setZero();

  CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0,x);
  Motion nu_next(Eigen::Map<Motion::Vector6>(nu_next_vec.data()));

  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);

  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),6,6);

  // Check using finite differencies
  Motion dv(Motion::Zero());
  typedef Eigen::Matrix<Scalar,6,6> Matrix6;
  Matrix6 Jlog6_fd(Matrix6::Zero());
  Motion v_plus, v0(log6(M));

  const Scalar eps = 1e-8;
  for(int k = 0; k < 6; ++k)
  {
    dv.toVector()[k] = eps;
    SE3 M_plus = M * exp6(dv);
    v_plus = log6(M_plus);
    Jlog6_fd.col(k) = (v_plus-v0).toVector()/eps;
    dv.toVector()[k] = 0;
  }

  SE3::Matrix6 Jlog6_analytic;
  pinocchio::Jlog6(M, Jlog6_analytic);

  BOOST_CHECK(Jlog6.isApprox(Jlog6_analytic));
  BOOST_CHECK(Jlog6_fd.isApprox(Jlog6,pinocchio::math::sqrt(eps)));
}

BOOST_AUTO_TEST_SUITE_END()


#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include "g2o/EXTERNAL/ceres/autodiff.h"
#include <g2o/core/auto_differentiation.h>

#include <g2o/core/robust_kernel_impl.h>
#include <iostream>

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

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

class VertexCamera: public g2o::BaseVertex<9, Eigen::Matrix<double, 9, 1>>      //  here the camera is parameterized as a 9-vector (angle axis, t, f, k1, k2)
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
        _estimate = Eigen::Matrix<double, 9, 1>::Zero();
    }

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

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

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

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

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

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

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

    EdgeReprojection() { }

    template <class T>
    bool operator() (const T* camera, const T* landmark, T* residuals) const
    {
        T Xc[3];
        ceres::AngleAxisRotatePoint(camera, landmark, Xc);
        Xc[0] += camera[3];
        Xc[1] += camera[4];
        Xc[2] += camera[5];

        T Xp[2];
        Xp[0] = Xc[0] / Xc[2];
        Xp[1] = Xc[1] / Xc[2];

        T n2 = Xp[0] * Xp[0] + Xp[1] * Xp[1];
        T r = T(1.0) + n2 * (camera[7] + n2 * camera[8]);

        T uv[2];
        uv[0] = -Xp[0] * camera[6] * r;
        uv[1] = -Xp[1] * camera[6] * r;

        residuals[0] = T(_measurement[0]) - uv[0];
        residuals[1] = T(_measurement[1]) - uv[1];
        return true;
    }

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

    G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff

};

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

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

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

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

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

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

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

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

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

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

    for (int i = 0; i < dataset.num_cameras(); ++i)
    {
        Eigen::Matrix<double, 9, 1> in = camera_vertices[i]->estimate();
        double *out = cameras + i * dataset.camera_block_size();
        for (int i = 0; i < 9; ++i)
        {
            out[i] = in[i];
        }
    }
    for (int i = 0; i < dataset.num_points(); ++i)
    {
        Eigen::Vector3d X = landmark_vertices[i]->estimate();
        landmarks[i*3] = X.x();
        landmarks[i*3+1] = X.y();
        landmarks[i*3+2] = X.z();
    }

    dataset.WriteToPLYFile("after_ba_g2o.ply");

    return 0;
}


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

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <random>
#include <cmath>
#include <boost/math/special_functions/jacobi.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using std::abs;
using boost::math::jacobi;
using boost::math::jacobi_derivative;

template<typename Real>
void test_to_quadratic()
{
    Real h = 1/Real(8);
    for (Real alpha = -1 + h; alpha < 2; alpha += h) {
        for (Real beta = -1 + h; beta < 2; beta += h) {
            for (Real x = -1; x < 1; x += h) {
                Real expected = 1;
                Real computed = jacobi(0, alpha, beta, x);
                CHECK_ULP_CLOSE(expected, computed, 0);

                expected = (alpha + 1) + (alpha + beta +2)*(x-1)/2;
                computed = jacobi(1, alpha, beta, x);
                CHECK_ULP_CLOSE(expected, computed, 0);

                expected = (alpha + 1)*(alpha+2)/2 + (alpha + 2)*(alpha + beta + 3)*(x-1)/2 + (alpha + beta + 3)*(alpha + beta + 4)*(x-1)*(x-1)/8;
                computed = jacobi(2, alpha, beta, x);
                CHECK_ULP_CLOSE(expected, computed, 1);

            }
        }
    }
}

template<typename Real>
void test_symmetry()
{
    Real h = 1/Real(4);
    for (Real alpha = -1 + h; alpha < 2; alpha += h) {
        for (Real beta = -1 + h; beta < 2; beta += h) {
            for (Real x = -1; x < 1; x += h) {
                for (size_t n = 0; n < 20; n += 2)
                {
                    Real expected = jacobi(n, beta, alpha , -x);
                    Real computed = jacobi(n, alpha, beta, x);
                    CHECK_ULP_CLOSE(expected, computed, 0);

                    expected = jacobi(n+1, beta, alpha, -x);
                    computed = -jacobi(n+1, alpha, beta, x);
                    CHECK_ULP_CLOSE(expected, computed, 0);
                }
            }
        }
    }
}

template<typename Real>
void test_derivative()
{
    Real h = 1/Real(4);
    for (Real alpha = -1 + h; alpha < 2; alpha += h) {
        for (Real beta = -1 + h; beta < 2; beta += h) {
            for (Real x = -1; x < 1; x += h) {
                Real expected = 0;
                Real computed = jacobi_derivative(0, alpha, beta, x, 1);
                CHECK_ULP_CLOSE(expected, computed, 0);

                expected = (alpha + beta + 2)/2;
                computed = jacobi_derivative(1, alpha, beta, x, 1);
                CHECK_ULP_CLOSE(expected, computed, 0);

                expected = (alpha + 2)*(alpha + beta + 3)/2 + (alpha + beta + 3)*(alpha + beta + 4)*(x-1)/4;
                computed = jacobi_derivative(2, alpha, beta, x, 1);
                CHECK_ULP_CLOSE(expected, computed, 0);

                expected = (alpha + beta + 3)*(alpha + beta + 4)/4;
                computed = jacobi_derivative(2, alpha, beta, x, 2);
                CHECK_ULP_CLOSE(expected, computed, 0);
            }
        }
    }
}

int main()
{
    test_to_quadratic<double>();
    test_to_quadratic<long double>();

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

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

#ifdef BOOST_HAS_FLOAT128
    test_to_quadratic<boost::multiprecision::float128>();
    test_symmetry<boost::multiprecision::float128>();
    test_derivative<boost::multiprecision::float128>();
#endif

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


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <ostream>

int main()
{
  namespace ublas = boost::numeric::ublas;
  using value   = float;
  using tensor = ublas::tensor_dynamic<value>;
  using matrix = ublas::matrix<value>;
  using vector = ublas::vector<value>;
  using shape   = tensor::extents_type;

  try {

    auto A = tensor{3,4,2};
    auto B = A = 2;

    // Calling overloaded operators
    // and using simple tensor expression templates.
    if( A != (B+1) ){
      A += 2*B - 1;
    }

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "A=" << A << ";" << std::endl << std::endl;

    auto n = shape{3,4};
    auto D = matrix(n[0],n[1],1);
    auto e = vector(n[1],1);
    auto f = vector(n[0],2);

    // Calling constructor with
    // vector expression templates
    tensor C = 2*f;
    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "C=" << C << ";" << std::endl << std::endl;

    // Calling overloaded operators
    // and mixing simple tensor and matrix expression templates
    tensor F = 3*C + 4*prod(2*D,e);

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "F=" << F << ";" << std::endl << std::endl;

  }  catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of simple expression." << std::endl;
  }

}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXcd X = MatrixXcd::Random(4,4);
MatrixXcd A = X + X.adjoint();
cout << "Here is a random self-adjoint 4x4 matrix:" << endl << A << endl << endl;

Tridiagonalization<MatrixXcd> triOfA(A);
MatrixXd T = triOfA.matrixT();
cout << "The tridiagonal matrix T is:" << endl << T << endl << endl;

cout << "We can also extract the diagonals of T directly ..." << endl;
VectorXd diag = triOfA.diagonal();
cout << "The diagonal is:" << endl << diag << endl;
VectorXd subdiag = triOfA.subDiagonal();
cout << "The subdiagonal is:" << endl << subdiag << endl;

  return 0;
}


#include <stan/math.hpp>
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <cmath>
#include <vector>

// This test fixture swallows output to std::cout
class Math : public ::testing::Test {
 public:
  void SetUp() {
    output_.str("");
    cout_backup_ = std::cout.rdbuf();
    std::cout.rdbuf(output_.rdbuf());
  }

  void TearDown() {
    std::cout.rdbuf(cout_backup_);
    stan::math::recover_memory();
  }

  std::stringstream output_;
  std::streambuf* cout_backup_;
};

TEST_F(Math, paper_example_1) {
  using std::pow;
  double y = 1.3;
  stan::math::var mu = 0.5, sigma = 1.2;

  stan::math::var lp = 0;
  lp -= 0.5 * log(2 * stan::math::pi());
  lp -= log(sigma);
  lp -= 0.5 * pow((y - mu) / sigma, 2);
  std::cout << "f(mu, sigma) = " << lp.val() << std::endl;

  lp.grad();
  std::cout << " d.f / d.mu = " << mu.adj()
            << " d.f / d.sigma = " << sigma.adj() << std::endl;
}

TEST_F(Math, paper_example_2) {
  double y = 1.3;
  stan::math::var mu = 0.5, sigma = 1.2;

  stan::math::var lp = 0;
  lp -= 0.5 * log(2 * stan::math::pi());
  lp -= log(sigma);
  lp -= 0.5 * pow((y - mu) / sigma, 2);

  std::vector<stan::math::var> theta;
  theta.push_back(mu);
  theta.push_back(sigma);
  std::vector<double> g;
  lp.grad(theta, g);
  std::cout << " d.f / d.mu = " << g[0] << " d.f / d.sigma = " << g[1]
            << std::endl;
}

namespace paper {  // paper_example_3
template <typename T1, typename T2, typename T3>
inline stan::return_type_t<T1, T2, T3> normal_log(const T1& y, const T2& mu,
                                                  const T3& sigma) {
  using std::log;
  using std::pow;
  return -0.5 * pow((y - mu) / sigma, 2.0) - log(sigma)
         - 0.5 * log(2 * stan::math::pi());
}
}  // namespace paper

TEST_F(Math, paper_example_3) {
  double y = 1.3;
  stan::math::var mu = 0.5, sigma = 1.2;

  stan::math::var lp = normal_log(y, mu, sigma);
  EXPECT_FLOAT_EQ(-1.323482, lp.val());
}

// paper_example_4: remove 'paper::' when including in the paper
namespace paper {
using Eigen::Dynamic;
using Eigen::Matrix;

struct normal_ll {
  const Matrix<double, Dynamic, 1> y_;

  explicit normal_ll(const Matrix<double, Dynamic, 1>& y) : y_(y) {}

  template <typename T>
  T operator()(const Matrix<T, Dynamic, 1>& theta) const {
    T mu = theta[0];
    T sigma = theta[1];
    T lp = 0;
    for (int n = 0; n < y_.size(); ++n)
      lp += paper::normal_log(y_[n], mu, sigma);
    return lp;
  }
};
}  // namespace paper

TEST_F(Math, paper_example_4) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using paper::normal_ll;

  Matrix<double, Dynamic, 1> y(3);
  y << 1.3, 2.7, -1.9;
  normal_ll f(y);

  Matrix<double, Dynamic, 1> theta(2);
  theta << 1.3, 2.9;

  double fx;
  Matrix<double, Dynamic, 1> grad_fx;
  stan::math::gradient(f, theta, fx, grad_fx);
}

namespace paper_example_5 {
using Eigen::Dynamic;
using Eigen::Matrix;

struct functor {
  const Matrix<double, Dynamic, 1> y_;

  explicit functor(const Matrix<double, Dynamic, 1>& y) : y_(y) {}

  template <typename T>
  Matrix<T, Dynamic, 1> operator()(const Matrix<T, Dynamic, 1>& theta) const {
    Matrix<T, Dynamic, 1> lp(y_.size());
    T mu = theta[0];
    T sigma = theta[1];
    for (int n = 0; n < y_.size(); ++n)
      lp[n] = paper::normal_log(y_[n], mu, sigma);
    return lp;
  }
};

}  // namespace paper_example_5

TEST_F(Math, paper_example_5) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::var;

  Matrix<double, Dynamic, 1> y(3);
  y << 1.3, 2.7, -1.9;
  paper_example_5::functor f(y);

  Matrix<double, Dynamic, 1> x(2);
  x << 1.3, 2.9;

  // paper_example_5 starts with the next line
  // Matrix<double, Dynamic, 1> x = ...;   // inputs

  Matrix<var, Dynamic, 1> x_var(x.size());
  for (int i = 0; i < x.size(); ++i)
    x_var(i) = x(i);

  Matrix<var, Dynamic, 1> f_x_var = f(x_var);

  Matrix<double, Dynamic, 1> f_x(f_x_var.size());
  for (int i = 0; i < f_x.size(); ++i)
    f_x(i) = f_x_var(i).val();

  Matrix<double, Dynamic, Dynamic> J(f_x_var.size(), x_var.size());
  for (int i = 0; i < f_x_var.size(); ++i) {
    if (i > 0)
      stan::math::set_zero_all_adjoints();
    f_x_var(i).grad();
    for (int j = 0; j < x_var.size(); ++j)
      J(i, j) = x_var(j).adj();
  }
}

TEST_F(Math, paper_example_6) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::var;

  Matrix<double, Dynamic, 1> y(3);
  y << 1.3, 2.7, -1.9;
  paper_example_5::functor f(y);

  Matrix<double, Dynamic, 1> x(2);
  x << 1.3, 2.9;

  // paper_example_6
  Matrix<double, Dynamic, Dynamic> J;
  Matrix<double, Dynamic, 1> f_x;
  stan::math::jacobian(f, x, f_x, J);
}


/**
 * @date Tue Jan 18 17:07:26 2011 +0100
 * @author André Anjos <andre.anjos@idiap.ch>
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * @brief Principal Component Analysis implemented with Singular Value
 * Decomposition or using the Covariance Method. Both are implemented using
 * LAPACK. Implementation.
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <algorithm>
#include <blitz/array.h>
#include <boost/format.hpp>
#include <bob.math/stats.h>
#include <bob.math/svd.h>
#include <bob.math/eig.h>

#include <bob.learn.linear/pca.h>

namespace bob { namespace learn { namespace linear {

  PCATrainer::PCATrainer(bool use_svd)
    : m_use_svd(use_svd), m_safe_svd(false)
  {
  }

  PCATrainer::PCATrainer(const PCATrainer& other)
    : m_use_svd(other.m_use_svd), m_safe_svd(other.m_safe_svd)
  {
  }

  PCATrainer::~PCATrainer() {}

  PCATrainer& PCATrainer::operator= (const PCATrainer& other) {
    if (this != &other) {
      m_use_svd = other.m_use_svd;
      m_safe_svd = other.m_safe_svd;
    }
    return *this;
  }

  bool PCATrainer::operator== (const PCATrainer& other) const {

    return m_use_svd == other.m_use_svd &&
      m_safe_svd == other.m_safe_svd;

  }

  bool PCATrainer::operator!= (const PCATrainer& other) const {

    return !(this->operator==(other));

  }

  /**
   * Sets up the machine calculating the PC's via the Covariance Matrix
   */
  static void pca_via_covmat(Machine& machine,
      blitz::Array<double,1>& eigen_values, const blitz::Array<double,2>& X,
      int rank) {

    /**
     * computes the covariance matrix (X-mu)(X-mu)^T / (len(X)-1) and then solves
     * the generalized eigen-value problem taking into consideration the
     * covariance matrix is symmetric (and, by extension, hermitian).
     */
    blitz::Array<double,1> mean(X.extent(1));
    blitz::Array<double,2> Sigma(X.extent(1), X.extent(1));
    bob::math::scatter_(X, Sigma, mean);
    Sigma /= (X.extent(0)-1); //unbiased variance estimator

    blitz::Array<double,2> U(X.extent(1), X.extent(1));
    blitz::Array<double,1> e(X.extent(1));
    bob::math::eigSym_(Sigma, U, e);
    e.reverseSelf(0);
    U.reverseSelf(1);

    /**
     * sets the linear machine with the results:
     */
    machine.setInputSubtraction(mean);
    machine.setInputDivision(1.0);
    machine.setBiases(0.0);
    if (e.size() == eigen_values.size()) {
      eigen_values = e;
      machine.setWeights(U);
    }
    else {
      eigen_values = e(blitz::Range(0,rank-1));
      machine.setWeights(U(blitz::Range::all(), blitz::Range(0,rank-1)));
    }

  }

  /**
   * Sets up the machine calculating the PC's via SVD
   */
  static void pca_via_svd(Machine& machine, blitz::Array<double,1>& eigen_values,
      const blitz::Array<double,2>& X, int rank, bool safe_svd) {

    // removes the empirical mean from the training data
    blitz::Array<double,2> data(X.extent(1), X.extent(0));
    blitz::Range a = blitz::Range::all();
    for (int i=0; i<X.extent(0); ++i) data(a,i) = X(i,a);

    // computes the mean of the training data
    blitz::secondIndex j;
    blitz::Array<double,1> mean(X.extent(1));
    mean = blitz::mean(data, j);

    // applies the training data mean
    for (int i=0; i<X.extent(0); ++i) data(a,i) -= mean;

    /**
     * computes the singular value decomposition using lapack
     *
     * note: Lapack already organizes the U,Sigma,V**T matrixes so that the
     * singular values in Sigma are organized by decreasing order of magnitude.
     * You **don't** need sorting after this.
     */
    const int rank_1 = (rank == (int)X.extent(1))? X.extent(1) : X.extent(0);
    blitz::Array<double,2> U(X.extent(1), rank_1);
    blitz::Array<double,1> sigma(rank_1);
    bob::math::svd_(data, U, sigma, safe_svd);

    /**
     * sets the linear machine with the results:
     *
     * note: eigen values are sigma^2/X.extent(0) diagonal
     *       eigen vectors are the rows of U
     */
    machine.setInputSubtraction(mean);
    machine.setInputDivision(1.0);
    machine.setBiases(0.0);
    blitz::Range up_to_rank(0, rank-1);
    machine.setWeights(U(a,up_to_rank));

    //weight normalization (if necessary):
    //norm_factor = blitz::sum(blitz::pow2(V(all,i)))

    // finally, we set also the eigen values in this version
    eigen_values = (blitz::pow2(sigma)/(X.extent(0)-1))(up_to_rank);
  }

  void PCATrainer::train(Machine& machine, blitz::Array<double,1>& eigen_values,
      const blitz::Array<double,2>& X) const {

    // data is checked now and conforms, just proceed w/o any further checks.
    const int rank = output_size(X);

    // Checks that the dimensions are matching
    if (machine.inputSize() != (size_t)X.extent(1)) {
      boost::format m("Number of features at input data set (%d columns) does not match machine input size (%d)");
      m % X.extent(1) % machine.inputSize();
      throw std::runtime_error(m.str());
    }
    if (machine.outputSize() != (size_t)rank) {
      boost::format m("Number of outputs of the given machine (%d) does not match the maximum covariance rank, i.e., min(#samples-1,#features) = min(%d, %d) = %d");
      m % machine.outputSize() % (X.extent(0)-1) % X.extent(1) % rank;
      throw std::runtime_error(m.str());
    }
    if (eigen_values.extent(0) != rank) {
      boost::format m("Number of eigenvalues on the given 1D array (%d) does not match the maximum covariance rank, i.e., min(#samples-1,#features) = min(%d,%d) = %d");
      m % eigen_values.extent(0) % (X.extent(0)-1) % X.extent(1) % rank;
      throw std::runtime_error(m.str());
    }

    if (m_use_svd) pca_via_svd(machine, eigen_values, X, rank, m_safe_svd);
    else pca_via_covmat(machine, eigen_values, X, rank);
  }

  void PCATrainer::train(Machine& machine, const blitz::Array<double,2>& X) const {
    blitz::Array<double,1> throw_away_eigen_values(output_size(X));
    train(machine, throw_away_eigen_values, X);
  }

  size_t PCATrainer::output_size (const blitz::Array<double,2>& X) const {
    return (size_t)std::min(X.extent(0)-1,X.extent(1));
  }

}}}


#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with ADDs and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
double wcre_add(const Cudd& mgr, const std::vector<BDD>& f,
              const std::vector<BDD>& f_hat,
              const abo::util::NumberRepresentation num_rep
                = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes bounds on the maximum relative value of f in relation to g
 * It is defined as the maximum of |f(x)| / max(1, |g(x)|) over all inputs x
 * This function returns a range in which the actual relative value is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum bound
 * returned by this function is at most twice the minimum bound
 * @param mgr The BDD object manager
 * @param f The function to compute the maximum relative value of (must be an unsigned integer)
 * @param g The function to use as a relation. Must have the same number of bits as f (must be an
 * unsigned integer)
 * @return {min, max}, the lower and upper bound on the maximum relative value
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
maximum_relative_value_bounds(const Cudd& mgr, const std::vector<BDD>& f,
                              const std::vector<BDD>& g);

/**
 * @brief Computes bounds on the maximum relative difference between f and f_hat
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * This function returns a range in which the actual maximum relative error is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum error
 * returned by this function is at most twice the minimum error
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return {min, max}, the lower and upper bound on the maximum relative error
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
    wcre_bounds(
        const Cudd& mgr,
        const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a binary search to find the maximum value
 * If the correct value is found, the search is aborted. Otherwise, it is run until the desired
 * precision is reached
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional bits used during the search to represent values
 * smaller than one
 * @param precision The desired precision of the result if the correct value is not found during the
 * binary search Do not set it lower than 2^-num_extra_bits
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
double wcre_search(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int num_extra_bits = 16,
        double precision = 0.0001,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a binary search to find the maximum value
 * If the correct value is found, the search is aborted. Otherwise, it is run until the desired
 * precision is reached
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param samples The number of random input samples drawn in each iteration
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs as a fraction [numerator, denominator]
 */
std::pair<long, long> wcre_randomized_search(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int samples = 1,
        const abo::util::NumberRepresentation num_rep
        = abo::util::NumberRepresentation::BaseTwo);
/**
 * @brief Computes the maximum relative difference between f and f_hat for any input
 * It is defined as the maximum of |f(x) - f_hat(x)| / max(1, |f(x)|) over all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a symbolic division and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional fixed precision bits to use during the division
 * As the result of each division is not an integer, the result is described as a fixed point number
 * with exactly num_extra_bits bits with a lower significance than one. Roughly correlates the the
 * precision of the result
 * @param num_rep The number representation for f and f_hat
 * @return the maximum relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100 wcre_symbolic_division(
        const Cudd& mgr, const std::vector<BDD>& f,
        const std::vector<BDD>& f_hat,
        unsigned int num_extra_bits = 16,
        const abo::util::NumberRepresentation num_rep
            = abo::util::NumberRepresentation::BaseTwo);

} // namespace abo::error_metrics


#include <Eigen/Core>
#include <iostream>
using namespace Eigen;
using namespace std;

// define a custom template binary functor
template<typename Scalar> struct MakeComplexOp {
  EIGEN_EMPTY_STRUCT_CTOR(MakeComplexOp)
  typedef complex<Scalar> result_type;
  complex<Scalar> operator()(const Scalar& a, const Scalar& b) const { return complex<Scalar>(a,b); }
};

int main(int, char**)
{
  Matrix4d m1 = Matrix4d::Random(), m2 = Matrix4d::Random();
  cout << m1.binaryExpr(m2, MakeComplexOp<double>()) << endl;
  return 0;
}


/**
 * @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
  // eval member function returns the lememt matrix for the finite elemet space and the bilinear form
  // in the weak formulation of 2.8.4
  Eigen::Matrix<double, 4,4> elment_mat_lap;
  Eigen::Matrix<double,4,4> element_mat_mass;

  // define the class
  lf::uscalfe::LinearFELaplaceElementMatrix laplace_elem_builder;
  element_mat_lap = laplace_elem_builder.Eval(*cell);

  // computations differ depending on the type of the cell
  case lf::base:RefEl::kTria():{
    double area = 0.5*((vertices(0,1)-vertices(0,0))*(vertices(1,2)-vertices(1,0)
    )-(vertices(1,1)-vertices(1,0))*(vertices(0,2)-vertices(0,0)));
    element_mat_mass >> 2.,1.,1.,0.,
     1.,2.,1.,0.,
     1.,1.,2.,0.,
     0.,0.,0.,0.;
    element_mat_mass = area/12*elemet_mat_mass;
    break;

  } case lf::base::RefEl::Kquad():{
    double area = (vertices(0,1)-vertices(0,0))*(vertices(1,3)-vertices(1,0));
    element_mat_mass >> 4.,2.,1.,2.,
    2.,4.,2.,1.,
    1.,2.,4.,2.,
    2.,1.,2.,4.;
    element_mat_mass = area/36*element_mat_mass;
    break;
  }

  elem_mat = element_mat_mass + element_mat_lap;

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

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


#pragma once

// Armadillo
#include <armadillo>

namespace mant {
  arma::mat hammersleySet(
      const arma::uvec& bases,
      const arma::uvec& seeds,
      const arma::uword numberOfElements);
  arma::mat hammersleySet(
      const arma::uvec& bases,
      const arma::uword numberOfElements);

  arma::mat haltonSequence(
      const arma::uvec& bases,
      const arma::uvec& seeds,
      const arma::uword numberOfElements);
  arma::mat haltonSequence(
      const arma::uvec& bases,
      const arma::uword numberOfElements);

  arma::vec vanDerCorputSequence(
      const arma::uword base,
      const arma::uword seed,
      const arma::uword numberOfElements);
  arma::vec vanDerCorputSequence(
      const arma::uword base,
      const arma::uword numberOfElements);
}