// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#if defined(_MSC_VER)
#  pragma warning( disable : 4101 )
#endif

#include <boost/timer.hpp>

#include <boost/concept/requires.hpp>
#include <boost/concept_check.hpp>
#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P1, typename P2>
void test_null_distance_3d()
{
    P1 p1;
    bg::assign_values(p1, 1, 2, 3);
    P2 p2;
    bg::assign_values(p2, 1, 2, 3);

    typedef bg::strategy::distance::pythagoras<> pythagoras_type;
    typedef typename bg::strategy::distance::services::return_type<pythagoras_type, P1, P2>::type return_type;

    pythagoras_type pythagoras;
    return_type result = pythagoras.apply(p1, p2);

    BOOST_CHECK_EQUAL(result, return_type(0));
}

template <typename P1, typename P2>
void test_axis_3d()
{
    P1 p1;
    bg::assign_values(p1, 0, 0, 0);
    P2 p2;
    bg::assign_values(p2, 1, 0, 0);

    typedef bg::strategy::distance::pythagoras<> pythagoras_type;
    typedef typename bg::strategy::distance::services::return_type<pythagoras_type, P1, P2>::type return_type;

    pythagoras_type pythagoras;

    return_type result = pythagoras.apply(p1, p2);
    BOOST_CHECK_EQUAL(result, return_type(1));

    bg::assign_values(p2, 0, 1, 0);
    result = pythagoras.apply(p1, p2);
    BOOST_CHECK_EQUAL(result, return_type(1));

    bg::assign_values(p2, 0, 0, 1);
    result = pythagoras.apply(p1, p2);
    BOOST_CHECK_CLOSE(result, return_type(1), 0.001);
}

template <typename P1, typename P2>
void test_arbitrary_3d()
{
    P1 p1;
    bg::assign_values(p1, 1, 2, 3);
    P2 p2;
    bg::assign_values(p2, 9, 8, 7);

    {
        typedef bg::strategy::distance::pythagoras<> strategy_type;
        typedef typename bg::strategy::distance::services::return_type<strategy_type, P1, P2>::type return_type;

        strategy_type strategy;
        return_type result = strategy.apply(p1, p2);
        BOOST_CHECK_CLOSE(result, return_type(10.77032961427), 0.001);
    }

    {
        // Check comparable distance
        typedef bg::strategy::distance::comparable::pythagoras<> strategy_type;
        typedef typename bg::strategy::distance::services::return_type<strategy_type, P1, P2>::type return_type;

        strategy_type strategy;
        return_type result = strategy.apply(p1, p2);
        BOOST_CHECK_EQUAL(result, return_type(116));
    }
}

template <typename P1, typename P2, typename CalculationType>
void test_services()
{
    namespace bgsd = bg::strategy::distance;
    namespace services = bg::strategy::distance::services;

    {

        // Compile-check if there is a strategy for this type
        typedef typename services::default_strategy
            <
                bg::point_tag, bg::point_tag, P1, P2
            >::type pythagoras_strategy_type;

        boost::ignore_unused<pythagoras_strategy_type>();
    }

    P1 p1;
    bg::assign_values(p1, 1, 2, 3);

    P2 p2;
    bg::assign_values(p2, 4, 5, 6);

    double const sqr_expected = 3*3 + 3*3 + 3*3; // 27
    double const expected = sqrt(sqr_expected); // sqrt(27)=5.1961524227

    // 1: normal, calculate distance:

    typedef bgsd::pythagoras<CalculationType> strategy_type;

    BOOST_CONCEPT_ASSERT( (bg::concepts::PointDistanceStrategy<strategy_type, P1, P2>) );

    typedef typename bgsd::services::return_type<strategy_type, P1, P2>::type return_type;

    strategy_type strategy;
    return_type result = strategy.apply(p1, p2);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 2: the strategy should return the same result if we reverse parameters
    result = strategy.apply(p2, p1);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 3: "comparable" to construct a "comparable strategy" for P1/P2
    //    a "comparable strategy" is a strategy which does not calculate the exact distance, but
    //    which returns results which can be mutually compared (e.g. avoid sqrt)

    // 3a: "comparable_type"
    typedef typename services::comparable_type<strategy_type>::type comparable_type;

    // 3b: "get_comparable"
    comparable_type comparable = bgsd::services::get_comparable<strategy_type>::apply(strategy);

    return_type c_result = comparable.apply(p1, p2);
    BOOST_CHECK_CLOSE(c_result, return_type(sqr_expected), 0.001);

    // 4: the comparable_type should have a distance_strategy_constructor as well,
    //    knowing how to compare something with a fixed distance
    return_type c_dist5 = services::result_from_distance<comparable_type, P1, P2>::apply(comparable, 5.0);
    return_type c_dist6 = services::result_from_distance<comparable_type, P1, P2>::apply(comparable, 6.0);

    // If this is the case:
    BOOST_CHECK(c_dist5 < c_result && c_result < c_dist6);

    // This should also be the case
    return_type dist5 = services::result_from_distance<strategy_type, P1, P2>::apply(strategy, 5.0);
    return_type dist6 = services::result_from_distance<strategy_type, P1, P2>::apply(strategy, 6.0);
    BOOST_CHECK(dist5 < result && result < dist6);
}

template <typename CoordinateType, typename CalculationType, typename AssignType>
void test_big_2d_with(AssignType const& x1, AssignType const& y1,
                 AssignType const& x2, AssignType const& y2)
{
    typedef bg::model::point<CoordinateType, 2, bg::cs::cartesian> point_type;
    typedef bg::strategy::distance::pythagoras<CalculationType> pythagoras_type;

    pythagoras_type pythagoras;
    typedef typename bg::strategy::distance::services::return_type<pythagoras_type, point_type, point_type>::type return_type;

    point_type p1, p2;
    bg::assign_values(p1, x1, y1);
    bg::assign_values(p2, x2, y2);
    return_type d = pythagoras.apply(p1, p2);

    /***
    std::cout << typeid(CalculationType).name()
        << " " << std::fixed << std::setprecision(20) << d
        << std::endl << std::endl;
    ***/

    BOOST_CHECK_CLOSE(d, return_type(1076554.5485833955678294387789057), 0.001);
}

template <typename CoordinateType, typename CalculationType>
void test_big_2d()
{
    test_big_2d_with<CoordinateType, CalculationType>
        (123456.78900001, 234567.89100001,
        987654.32100001, 876543.21900001);
}

template <typename CoordinateType, typename CalculationType>
void test_big_2d_string()
{
    test_big_2d_with<CoordinateType, CalculationType>
        ("123456.78900001", "234567.89100001",
        "987654.32100001", "876543.21900001");
}

template <typename CoordinateType>
void test_integer(bool check_types)
{
    typedef bg::model::point<CoordinateType, 2, bg::cs::cartesian> point_type;

    point_type p1, p2;
    bg::assign_values(p1, 12345678, 23456789);
    bg::assign_values(p2, 98765432, 87654321);

    typedef bg::strategy::distance::pythagoras<> pythagoras_type;
    typedef typename bg::strategy::distance::services::comparable_type
        <
            pythagoras_type
        >::type comparable_type;

    typedef typename bg::strategy::distance::services::return_type
        <
            pythagoras_type, point_type, point_type
        >::type distance_type;
    typedef typename bg::strategy::distance::services::return_type
        <
            comparable_type, point_type, point_type
        >::type cdistance_type;

    pythagoras_type pythagoras;
    distance_type distance = pythagoras.apply(p1, p2);
    BOOST_CHECK_CLOSE(distance, 107655455.02347542, 0.001);

    comparable_type comparable;
    cdistance_type cdistance = comparable.apply(p1, p2);
    BOOST_CHECK_EQUAL(cdistance, 11589696996311540.0);

    distance_type distance2 = sqrt(distance_type(cdistance));
    BOOST_CHECK_CLOSE(distance, distance2, 0.001);

    if (check_types)
    {
        BOOST_CHECK((boost::is_same<distance_type, double>::type::value));
        // comparable_distance results in now double too, obviously because
        // comp.distance point-segment can be fraction, even for integer input
        BOOST_CHECK((boost::is_same<cdistance_type, double>::type::value));
    }
}

template <typename P1, typename P2>
void test_all_3d()
{
    test_null_distance_3d<P1, P2>();
    test_axis_3d<P1, P2>();
    test_arbitrary_3d<P1, P2>();
}

template <typename P>
void test_all_3d()
{
    test_all_3d<P, int[3]>();
    test_all_3d<P, float[3]>();
    test_all_3d<P, double[3]>();
    test_all_3d<P, test::test_point>();
    test_all_3d<P, bg::model::point<int, 3, bg::cs::cartesian> >();
    test_all_3d<P, bg::model::point<float, 3, bg::cs::cartesian> >();
    test_all_3d<P, bg::model::point<double, 3, bg::cs::cartesian> >();
}

template <typename P, typename Strategy>
void time_compare_s(int const n)
{
    boost::timer t;
    P p1, p2;
    bg::assign_values(p1, 1, 1);
    bg::assign_values(p2, 2, 2);
    Strategy strategy;
    typename bg::strategy::distance::services::return_type<Strategy, P, P>::type s = 0;
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            bg::set<0>(p2, bg::get<0>(p2) + 0.001);
            s += strategy.apply(p1, p2);
        }
    }
    std::cout << "s: " << s << " t: " << t.elapsed() << std::endl;
}

template <typename P>
void time_compare(int const n)
{
    time_compare_s<P, bg::strategy::distance::pythagoras<> >(n);
    time_compare_s<P, bg::strategy::distance::comparable::pythagoras<> >(n);
}

int test_main(int, char* [])
{
    test_integer<int>(true);
    test_integer<boost::long_long_type>(true);
    test_integer<double>(false);

    test_all_3d<int[3]>();
    test_all_3d<float[3]>();
    test_all_3d<double[3]>();

    test_all_3d<test::test_point>();

    test_all_3d<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_all_3d<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_all_3d<bg::model::point<double, 3, bg::cs::cartesian> >();

    test_big_2d<float, float>();
    test_big_2d<double, double>();
    test_big_2d<long double, long double>();
    test_big_2d<float, long double>();

    test_services<bg::model::point<float, 3, bg::cs::cartesian>, double[3], long double>();
    test_services<double[3], test::test_point, float>();

    // TODO move this to another non-unit test
    // time_compare<bg::model::point<double, 2, bg::cs::cartesian> >(10000);

    return 0;
}


/**
 * @file stableevaluationatapoint_test.cc
 * @brief NPDE homework StableEvaluationAtAPoint
 * @author Amélie Loher
 * @date 29/04/2020
 * @copyright Developed at ETH Zurich
 */

#include "../stableevaluationatapoint.h"

#include <gtest/gtest.h>
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

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

TEST(StableEvaluationAtAPoint, PSL) {
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR "/../../meshes/square.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  const auto u = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };

  const Eigen::Vector2d x(0.3, 0.4);

  const double val = StableEvaluationAtAPoint::PSL(mesh_p, u, x);

  const double ref_val = 0.15525;

  double tol = 1.e-4;

  ASSERT_NEAR(std::abs(ref_val - val), 0.0, tol);
}

TEST(StableEvaluationAtAPoint, PDL) {
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR "/../../meshes/square.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  const auto u = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };

  const Eigen::Vector2d x(0.3, 0.4);

  const double val = StableEvaluationAtAPoint::PDL(mesh_p, u, x);

  const double ref_val = -0.484226;

  double tol = 1.e-4;

  ASSERT_NEAR(std::abs(ref_val - val), 0.0, tol);
}

TEST(StableEvaluationAtAPoint, PointEval) {
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR "/../../meshes/square.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  double error = StableEvaluationAtAPoint::PointEval(mesh_p);

  double ref_error = 0.0784387;

  double tol = 1.e-4;

  ASSERT_NEAR(std::abs(ref_error - error), 0.0, tol);
}

TEST(StableEvaluationAtAPoint, Jstar) {
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR
                                 "/../../meshes/square7.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  const auto u = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };

  lf::mesh::utils::MeshFunctionGlobal mf_u{u};
  Eigen::VectorXd uFE = lf::fe::NodalProjection(*fe_space, mf_u);

  const Eigen::Vector2d x(0.3, 0.4);

  double val = StableEvaluationAtAPoint::Jstar(fe_space, uFE, x);

  double ref_val = u(x);

  double tol = 1.e-2;

  ASSERT_NEAR(val, ref_val, tol);
}

/*
TEST(StableEvaluationAtAPoint, stab_pointEval) {
  auto mesh_factory_init = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader_init(std::move(mesh_factory_init),
                                 CURRENT_SOURCE_DIR "/../../meshes/square.msh");
  std::shared_ptr<lf::mesh::Mesh> mesh_p = reader_init.mesh();

  std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  const auto u = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };

  const Eigen::Vector2d x(0.3, 0.4);

  double val = StableEvaluationAtAPoint::StablePointEvaluation(fe_space, u, x);

  double ref_val = 0.0;

  double tol = 1.e-4;

  ASSERT_NEAR(std::abs(ref_val - val), 0.0, tol);
}
*/


//
//  functions.hpp
//  spreadsheet
//
//  Created by Irit Katriel on 05/08/2012.
//  Copyright (c) 2012 __MyCompanyName__. All rights reserved.
//

#pragma once

#include "streamulus.h"

#include <boost/type_traits.hpp>
#include <boost/utility/enable_if.hpp>

namespace spreadsheet {

namespace bm = boost::mpl;

template<typename F, typename ARG>
struct StreamifyType
{
    typedef typename bp::result_of::make_expr<bp::tag::function, F, ARG const &>::type type;
};

struct ABS_FUNC
{
    template<class Sig> struct result;

    template<class This,typename A>
    struct result<This(A)>
    {
        typedef A type;
    };

    template<typename A>
    typename boost::enable_if<boost::is_integral<A>, typename result<ABS_FUNC(A)>::type>::type
    operator()(const A& value) const
    {
        return std::abs(value);
    }

    template<typename A>
    typename boost::enable_if<boost::is_floating_point<A>, typename result<ABS_FUNC(A)>::type>::type
    operator()(const A& value) const
    {
        return std::fabs(value);
    }
};

template<double FUNC(double)>
struct DOUBLE_TO_DOUBLE_FUNC
{
    template<class Sig> struct result;

    template<class This,typename A>
    struct result<This(A)>
    {
        typedef A type;
    };

    template<typename A>
    typename result<DOUBLE_TO_DOUBLE_FUNC<FUNC>(A)>::type
    operator()(const A& value) const
    {
        return FUNC(value);
    }
};

#define LAZY_FUNC1(NAME, FUNC) \
template<typename A> \
typename StreamifyType<FUNC,A>::type \
NAME(const A& arg) \
{ \
    return streamulus::Streamify<FUNC>(arg); \
}

LAZY_FUNC1(ABS,   ABS_FUNC);
LAZY_FUNC1(ACOS,  DOUBLE_TO_DOUBLE_FUNC<std::acos >);
LAZY_FUNC1(ASIN,  DOUBLE_TO_DOUBLE_FUNC<std::asin >);
LAZY_FUNC1(ATAN,  DOUBLE_TO_DOUBLE_FUNC<std::atan >);
LAZY_FUNC1(CEIL,  DOUBLE_TO_DOUBLE_FUNC<std::ceil >);
LAZY_FUNC1(COS,   DOUBLE_TO_DOUBLE_FUNC<std::cos  >);
LAZY_FUNC1(COSH,  DOUBLE_TO_DOUBLE_FUNC<std::cosh >);
LAZY_FUNC1(EXP,   DOUBLE_TO_DOUBLE_FUNC<std::exp  >);
LAZY_FUNC1(FLOOR, DOUBLE_TO_DOUBLE_FUNC<std::floor>);
LAZY_FUNC1(LOG,   DOUBLE_TO_DOUBLE_FUNC<std::log  >);
LAZY_FUNC1(LOG10, DOUBLE_TO_DOUBLE_FUNC<std::log10>);
LAZY_FUNC1(SIN,   DOUBLE_TO_DOUBLE_FUNC<std::sin  >);
LAZY_FUNC1(SINH,  DOUBLE_TO_DOUBLE_FUNC<std::sinh >);
LAZY_FUNC1(SQRT,  DOUBLE_TO_DOUBLE_FUNC<std::sqrt >);
LAZY_FUNC1(TAN,   DOUBLE_TO_DOUBLE_FUNC<std::tan  >);
LAZY_FUNC1(TANH,  DOUBLE_TO_DOUBLE_FUNC<std::tanh >);

#undef LAZY_FUNC1

} // ns spreadsheet


// This file is part of the dune-xt project:
//   https://zivgitlab.uni-muenster.de/ag-ohlberger/dune-community/dune-xt
// Copyright 2009-2021 dune-xt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2017)
//   René Fritze     (2017 - 2020)
//   Tobias Leibner  (2020)

#ifndef DUNE_XT_COMMON_VECTOR_STATISTICS_HH
#define DUNE_XT_COMMON_VECTOR_STATISTICS_HH

#include <algorithm>

#include <dune/xt/common/type_traits.hh>
#include <dune/xt/common/vector.hh>

#include <boost/accumulators/statistics/variance.hpp>

namespace Dune::XT::Common {

template <class VectorType>
typename std::enable_if<is_vector<VectorType>::value, typename VectorAbstraction<VectorType>::S>::type
standard_deviation(const VectorType& vector)
{
  using namespace boost::accumulators;
  using FieldType = typename VectorAbstraction<VectorType>::S;
  accumulator_set<FieldType, stats<tag::lazy_variance>> acc;
  std::for_each(vector.begin(), vector.end(), [&](FieldType value) { acc(value); });
  return std::sqrt(variance(acc));
}

} // namespace Dune::XT::Common

#endif // DUNE_XT_COMMON_VECTOR_STATISTICS_HH


//
//  main.cpp
//  using_eigen
//
//  Created by on 2018/11/6.
//  Copyright @2018 kouui. All rights reserved.
//

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

int main()
{
    // Dynamic
    // X : unknown size, resizable
    // d : double
    Eigen::MatrixXd m;

    // fixed Sized Matrix
    Eigen::Matrix3d f;

    f << 1, 2, 3,
        4, 5, 6,
        7, 8, 9;

    f = Eigen::Matrix3d::Constant(1.0);

    m = Eigen::MatrixXd::Constant(5, 5, 1.0);

    std::cout << m << "\n";
}


// BLAS level 2 -- complex numbers

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::vector<cmplx_t> vct_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> m_t;
#else
typedef ublas::vector<cmplx_t, std::vector<cmplx_t> > vct_t;
typedef ublas::matrix<cmplx_t, ublas::column_major, std::vector<cmplx_t> > m_t;
#endif

int main() {

  cout << endl;

  vct_t vx (2);
  atlas::set (cmplx_t (1., 0.), vx);
  print_v (vx, "vx");
  vct_t vy (4); // vector size can be larger
                // than corresponding matrix size
  atlas::set (cmplx_t (0., 0.), vy);
  print_v (vy, "vy");
  cout << endl;

  m_t m (3, 2);
  init_m (m, kpp (1));
  print_m (m, "m");
  cout << endl;

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, m, vx, 0.0, vy);
  print_v (vy, "m vx");
  atlas::gemv (m, vx, vy);
  print_v (vy, "m vx");
  cout << endl;

  m (0, 0) = cmplx_t (0., 1.);
  m (0, 1) = cmplx_t (0., 2.);
  m (1, 0) = cmplx_t (0., 3.);
  m (1, 1) = cmplx_t (0., 4.);
  m (2, 0) = cmplx_t (0., 5.);
  m (2, 1) = cmplx_t (0., 6.);
  print_m (m, "m");
  cout << endl;

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, m, vx, 0.0, vy);
  print_v (vy, "m vx");
  atlas::gemv (m, vx, vy);
  print_v (vy, "m vx");
  cout << endl;

  m (0, 0) = cmplx_t (-1., 1.);
  m (0, 1) = cmplx_t (-2., 2.);
  m (1, 0) = cmplx_t (-3., 3.);
  m (1, 1) = cmplx_t (-4., 4.);
  m (2, 0) = cmplx_t (-5., 5.);
  m (2, 1) = cmplx_t (-6., 6.);
  print_m (m, "m");
  cout << endl;

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, m, vx, 0.0, vy);
  print_v (vy, "m vx");
  atlas::gemv (m, vx, vy);
  print_v (vy, "m vx");
  cout << endl;

  atlas::set (cmplx_t (0., 1.), vx);
  print_v (vx, "vx");

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, m, vx, 0.0, vy);
  print_v (vy, "m vx");
  atlas::gemv (m, vx, vy);
  print_v (vy, "m vx");
  cout << endl;

  atlas::set (cmplx_t (1., 1.), vx);
  print_v (vx, "vx");

  // vy = m vx
  atlas::gemv (CblasNoTrans, 1.0, m, vx, 0.0, vy);
  print_v (vy, "m vx");
  atlas::gemv (m, vx, vy);
  print_v (vy, "m vx");
  cout << endl;

  // vx = m^H vy
  atlas::set (cmplx_t (-1., -1.), vy);
  print_v (vy, "vy");
  atlas::gemv (CblasConjTrans, 1.0, m, vy, 0.0, vx);
  print_v (vx, "m^H vy");
  cout << endl;

  m_t mx (2, 2);
  m_t my (3, 2);

  ublas::matrix_column<m_t> mxc0 (mx, 0), mxc1 (mx, 1);
  ublas::matrix_column<m_t> myc0 (my, 0), myc1 (my, 1);

  atlas::set (cmplx_t (1., 0.), mxc0);
  atlas::set (cmplx_t (0., 0.), mxc1);
  atlas::set (cmplx_t (0., 0.), myc0);
  atlas::set (cmplx_t (0., 0.), myc1);
  print_m (mx, "mx");
  cout << endl;
  print_m (my, "my");
  cout << endl;

  // my[.,0] = m mx[.,0]
  atlas::gemv (m, mxc0, myc0);
  print_m (my, "m mx[.,0]");

  cout << endl;

}


#include <Eigen/Core>
#include "iris/iris.h"
#include "test_util.h"

int main() {
  // A simple example with a sphere centered at the origin
  Eigen::MatrixXd C(2,2);
  C << 1, 0,
       0, 1;
  Eigen::VectorXd d(2);
  d << 0, 0;
  iris::Ellipsoid ellipsoid(C, d);

  Eigen::MatrixXd obs(2,4);
  obs << 2, 3, 3, 2,
         2, 2, 3, 3;
  std::vector<Eigen::MatrixXd> obstacles;
  obstacles.push_back(obs);

  iris::Polyhedron result(2);
  bool infeasible_start;
  iris::separating_hyperplanes(obstacles, ellipsoid, result, infeasible_start);
  valuecheck(infeasible_start, false);
  Eigen::MatrixXd A_expected(1, 2);
  A_expected << 1/std::sqrt(2), 1/std::sqrt(2);
  Eigen::VectorXd b_expected(1);
  b_expected << 2.0 * 2.0 * 1.0/std::sqrt(2);
  valuecheckMatrix(result.getA(), A_expected, 1e-6);
  valuecheckMatrix(result.getB(), b_expected, 1e-6);

  return 0;
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <complex>
#include <tuple>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/splitting.h"
#include "vlasovpp/lagrange5.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

#define ping(X) std::cerr << __LINE__ << " " << #X << ":" << X << std::endl
int debug = 0;

auto
maxwellian ( double rho , double u , double T ) {
  //std::cout << rho << " " << u << " " << T << std::endl;
  //std::cout << rho/(std::sqrt(2.*math::pi<double>()*T)) << std::endl;
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int main(int,char**)
{
  std::size_t Nx = 135, Nv = 256;

  // $(u_c,E,\hat{f}_h)$ and $f_h$
  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,-1.76);
  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

  const double Kx = 0.5;
  // phase-space domain
  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();

  // compute dx, dv
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/Nv;
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/Nx;

  const double dt = 0.1;//1.*fh.step.dv;
  double Tf = 60.*dt;

  // velocity and frequency
  ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  const double l = fh.range.x_max-fh.range.x_min;
  ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  // initial condition
  double ui=2., alpha=0.2;
  auto tb_M1 = maxwellian(0.5*alpha,ui,1.) , tb_M2 = maxwellian(0.5*alpha,-ui,1.);
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] = std::cos(2.*math::pi<double>()/16.*(Vk(k)-0.5));
    }
    fft::fft(&(fh[k][0]),&(fh[k][Nx-1])+1,&(hfh[k][0]));
  }
  fh.write("vphl/split/init.dat");

  splitting<double,1> Lie( fh , l , 1. );

  std::vector<double> times;

  std::size_t i_t = 0;
  double current_time = 0.;
  while ( i_t < 60 ) {
    std::cout << " [" << std::setw(5) << i_t << "] " << i_t*dt << "\r" << std::flush;

    Lie.phi_b(dt,uc,E,hfh);

    current_time += dt;
    ++i_t;
    times.push_back( current_time );
  } // while current_time < Tf
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<< "    "<<std::endl;

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][Nx-1])+1,&(fh[k][0])); }
  fh.write("vphl/split/vp.dat");

  for ( auto k=0 ; k<fh.size(0) ; ++k ) {
    for ( auto i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] -= std::cos(2.*math::pi<double>()/16.*( Vk(k)-0.5-E(i)*Tf ));
    }
  }

  fh.write("vphl/split/diff.dat");

  for ( auto ei : E ) {
    std::cout << ei << " , ";
  }
  std::cout << std::endl;
  return 0;
}


#include <NTL/vec_RR.h>

NTL_START_IMPL

void InnerProduct(RR& xx, const vec_RR& a, const vec_RR& b)
{
   RR t1, x;

   long n = min(a.length(), b.length());
   long i;

   clear(x);
   for (i = 1; i <= n; i++) {
      mul(t1, a(i), b(i));
      add(x, x, t1);
   }

   xx = x;
}

void mul(vec_RR& x, const vec_RR& a, const RR& b_in)
{
   RR b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void mul(vec_RR& x, const vec_RR& a, double b_in)
{
   NTL_THREAD_LOCAL static RR b;
   conv(b, b_in);
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void add(vec_RR& x, const vec_RR& a, const vec_RR& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector add: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      add(x[i], a[i], b[i]);
}

void sub(vec_RR& x, const vec_RR& a, const vec_RR& b)
{
   long n = a.length();
   if (b.length() != n) LogicError("vector sub: dimension mismatch");
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      sub(x[i], a[i], b[i]);
}

void clear(vec_RR& x)
{
   long n = x.length();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

void negate(vec_RR& x, const vec_RR& a)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      negate(x[i], a[i]);
}

long IsZero(const vec_RR& a)
{
   long n = a.length();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

vec_RR operator+(const vec_RR& a, const vec_RR& b)
{
   vec_RR res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_RR, res);
}

vec_RR operator-(const vec_RR& a, const vec_RR& b)
{
   vec_RR res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_RR, res);
}

vec_RR operator-(const vec_RR& a)
{
   vec_RR res;
   negate(res, a);
   NTL_OPT_RETURN(vec_RR, res);
}

RR operator*(const vec_RR& a, const vec_RR& b)
{
   RR res;
   InnerProduct(res, a, b);
   return res;
}

void VectorCopy(vec_RR& x, const vec_RR& a, long n)
{
   if (n < 0) LogicError("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) ResourceError("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   long i;

   for (i = 0; i < m; i++)
      x[i] = a[i];

   for (i = m; i < n; i++)
      clear(x[i]);
}

NTL_END_IMPL


// (C) Copyright Jeremy Siek 2001.
// 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)

#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/graph_utility.hpp>

/*
  Sample output:
  isomorphic? 1
  f: 9 10 11 0 1 3 2 4 6 8 7 5
 */

int main()
{
    using namespace boost;

    const int n = 12;

    typedef adjacency_list< vecS, listS, undirectedS,
        property< vertex_index_t, int > >
        graph_t;
    graph_t g1(n), g2(n);

    std::vector< graph_traits< graph_t >::vertex_descriptor > v1(n), v2(n);

    property_map< graph_t, vertex_index_t >::type v1_index_map
        = get(vertex_index, g1),
        v2_index_map = get(vertex_index, g2);

    graph_traits< graph_t >::vertex_iterator i, end;
    int id = 0;
    for (boost::tie(i, end) = vertices(g1); i != end; ++i, ++id)
    {
        put(v1_index_map, *i, id);
        v1[id] = *i;
    }
    id = 0;
    for (boost::tie(i, end) = vertices(g2); i != end; ++i, ++id)
    {
        put(v2_index_map, *i, id);
        v2[id] = *i;
    }
    add_edge(v1[0], v1[1], g1);
    add_edge(v1[1], v1[2], g1);
    add_edge(v1[0], v1[2], g1);
    add_edge(v1[3], v1[4], g1);
    add_edge(v1[4], v1[5], g1);
    add_edge(v1[5], v1[6], g1);
    add_edge(v1[6], v1[3], g1);
    add_edge(v1[7], v1[8], g1);
    add_edge(v1[8], v1[9], g1);
    add_edge(v1[9], v1[10], g1);
    add_edge(v1[10], v1[11], g1);
    add_edge(v1[11], v1[7], g1);

    add_edge(v2[9], v2[10], g2);
    add_edge(v2[10], v2[11], g2);
    add_edge(v2[11], v2[9], g2);
    add_edge(v2[0], v2[1], g2);
    add_edge(v2[1], v2[3], g2);
    add_edge(v2[3], v2[2], g2);
    add_edge(v2[2], v2[0], g2);
    add_edge(v2[4], v2[5], g2);
    add_edge(v2[5], v2[7], g2);
    add_edge(v2[7], v2[8], g2);
    add_edge(v2[8], v2[6], g2);
    add_edge(v2[6], v2[4], g2);

    std::vector< graph_traits< graph_t >::vertex_descriptor > f(n);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    bool ret = isomorphism(g1, g2,
        make_iterator_property_map(f.begin(), v1_index_map, f[0]),
        degree_vertex_invariant(), get(vertex_index, g1),
        get(vertex_index, g2));
#else
    bool ret = isomorphism(g1, g2,
        isomorphism_map(
            make_iterator_property_map(f.begin(), v1_index_map, f[0])));
#endif
    std::cout << "isomorphic? " << ret << std::endl;

    std::cout << "f: ";
    for (std::size_t v = 0; v != f.size(); ++v)
        std::cout << get(get(vertex_index, g2), f[v]) << " ";
    std::cout << std::endl;

    return 0;
}


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
typedef Vector<u_char,Dynamic> ImgLine;
typedef Vector<double,Dynamic> TempImgLine;
//typedef Matrix<ImgLine,Dynamic,Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b){
    if (a<b) return b;
    else return a;
}

unsigned char min(u_char a, u_char b){
    if (a>b) return b;
    else return a;
}

double max(double a, double b){
    if (a<b) return b;
    else return a;
}

double min(double a, double b){
    if (a>b) return b;
    else return a;
}

int max(int a, int b){
    if (a<b) return b;
    else return a;
}

int min(int a, int b){
    if (a>b) return b;
    else return a;
}

// auto max(auto a, auto b){
//     if (a<b) return b;
//     else return a;
// }

// auto min(auto a, auto b){
//     if (a>b) return b;
//     else return a;
// }

vector<double> projectOnEigenSpace(vector<TempImgLine> eigenfaces, TempImgLine imToProj,int K){
    vector<double> res = vector<double>();
    for (int i=0;i<K;i++){
        res.push_back(eigenfaces[i].dot(imToProj));
    }
    return res;
}

ImgLine octToVec(OCTET* im, int nH, int nW){
    ImgLine res(nH*nW);
    for (int i=0; i<nH*nW;i++){
        res(i)=im[i];
    }
    return res;
}

double eigenProjsDistance(vector<double> proj1, vector<double> proj2){

    double res =0.0;
    for (int i=0; i<min(proj1.size(),proj2.size());i++){
        res+=(proj1[i]-proj2[i])*(proj1[i]-proj2[i]);
    }
    return res;
}

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

    //DB file :
    ofstream outFile;
    outFile.open ("DBEigen.txt");

    //recup eigenfaces

    if (argc<3){
        cout<<"usage : eigenfaces dir0 ... dirn\n"<<endl;
    }
    vector<string> directories;
    for (int i=0; i<argc-2;i++){
        string s = string(argv[i+2]);
        int found = s.find_last_of('/');

        directories.push_back(s.substr(found+1));
    }

    vector<TempImgLine> eigenfaces;
    int K =42; int nH;int nW;

    for (int i=0; i<K;i++){
        OCTET* im;
        char name[100];

        sprintf(name,"/im%d.pgm",i);
        string eigenFolder = string(string(argv[1])+string(name));

        lire_nb_lignes_colonnes_image_pgm(eigenFolder.c_str(),&nH,&nW);
        allocation_tableau(im,OCTET,nH*nW);
        lire_image_pgm(eigenFolder.c_str(),im,nH*nW);
        TempImgLine eigenFace(nH*nW) ;
        double sum=0.0;
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=im[j]-127;
            sum+=(double)(im[j]-127)*(double)(im[j]-127);
        }
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=eigenFace(j)/sqrt(sum);
            }
        eigenfaces.push_back(eigenFace);
        free(im);
    }

    //vector<vector<vector<double>>> registre = vector<vector<vector<double>>>();

    int countDirs =0;
    for (int i=0; i<argc-2;i++){
        int countFile=0;

        outFile<<"!"<<directories[i]<<endl;
        for (auto& file : std::filesystem::directory_iterator(argv[i+2])){
            //registre.push_back(vector<vector<double>>());
            OCTET* img;
            lire_nb_lignes_colonnes_image_pgm(file.path().c_str(),&nH,&nW);
            TempImgLine imgLine(nH*nW);
            allocation_tableau(img,OCTET, nH*nW);
            lire_image_pgm(file.path().c_str(),img,nH*nW);
            double sumIm=0.0;
            for (int ip=0;ip<nH*nW;ip++){sumIm+=(double)(img[ip]-127)*(double)(img[ip]-127);}
            for (int ip=0; ip<nH*nW;ip++){imgLine(ip)=(double)(img[ip]-127)/sumIm;}

            //registre[countDirs].push_back(projectOnEigenSpace(eigenfaces,imgLine,K));
            vector<double> testPrint = projectOnEigenSpace(eigenfaces,imgLine,K);
            for (int d=0; d<testPrint.size();d++){outFile<<testPrint[d]<<" ";}
            outFile<<endl;

            countFile++;
        }
        outFile<<endl;
        countDirs++;
    }

    outFile.close();
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      GTOP, http://www.esa.int/gsp/ACT/doc/INF/Code/globopt/GTOPtoolbox.rar.
 *
 *    Notes
 *      Note that for some of the near-parabolic cases, the tolerance used for the to-and-fro
 *      conversions (Test 4) is several order of magnitudes higher than used for the regular cases.
 *      This should be investigated further in the future to fully characterize the nature of the
 *      conversions in the near-parabolic cases.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <fstream>

#include "Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace orbital_element_conversions;
using namespace mathematical_constants;

//! Error writing function.
/*!
 * This function writes the input values that led to errors to a unique file, if any errors occured
 * during the random tests. To make the file unique, the date and time of execution is added. An
 * error message also shows the location of the file.
 * \param eccentricities A vector containing the eccentricities that caused a failure.
 * \param meanAnomalies A vector containing the mean anomalies that caused a failure.
 * \param testName A string specifying the name of the test that failed.
 */
void writeErrorsToFile( std::vector< double > eccentricities, std::vector< double > meanAnomalies,
                        std::string testName )
{
    // Obtain the current time.
    const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time( );

    // Make a string containing the output file name. This output file is tagged with the date and
    // time at which the code was executed. The default date format is: YYYYMMDDTHHMMSS, in which T
    // separates date and time.
    const std::string outputFileName = input_output::getTudatRootPath( ) +
            "Astrodynamics/BasicAstrodynamics/UnitTests/" +
            "ErrorReportConversionMeanToHyperbolicEccentricAnomaly" +
            testName + "RunAt" + boost::posix_time::to_iso_string( now )
            + ".txt";

    // Make a stream to a file.
    std::ofstream errorFile( outputFileName.c_str( ) );

    // Write an introduction in the file explaining what happened. 70 lines long.
    errorFile << "This error report was generated because the unit test for the" << std::endl
              << "conversion of mean to hyperbolic eccentric anomaly has failed in" << std::endl
              << "one of the random tests. To ensure the data for which it failed is" << std::endl
              << "not lost, the corresponding input variables for these cases are" << std::endl
              << "listed below. Please report a bug on the Tudat website " << std::endl
              << "(tudat.tudelft.nl), with these values, so that someone will look" << std::endl
              << "into it and the code can be improved." << std::endl << std::endl
              << "Eccentricities:           Mean anomalies:" << std::endl;

    // Set the precision for the output of the variables to 16 digits.
    errorFile.precision( 16 );

    // Add the corresponding eccentricities and mean anomalies at neatly arranged positions.
    for ( unsigned int counter = 0; counter < eccentricities.size( ); counter++ )
    {
        errorFile << std::setw( 25 ) << eccentricities[ counter ]
                  << std::setw( 25 ) << meanAnomalies[ counter ] << std::endl;
    }
    errorFile.close( );

    // Add an error message specifying the file that the values have been written to.
    std::cerr << "One or multiple errors occurred during random sampling. " << std::endl
              << "The values leading to these errors have been written to the following file: "
              << std::endl << outputFileName;
}

BOOST_AUTO_TEST_SUITE( test_mean_to_hyperbolic_eccentric_anomaly_conversion )

//! Test 1: Test a range of values for the conversion.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToHyperbolicEccentricAnomaly_range )
{
    // Set array of test values for eccentricity.
    const double arrayOfTestEccentricities [ 6 ] = { 1.03, 1.28, 1.97, 2.56, 10.87, 99.72 };

    // Set array of test values for mean anomaly.
    const double arrayOfTestMeanAnomalies [ 6 ] = { 1.5, 6.0, 0.5, -4.0, 5.5, 2.5 };

    // Set array of expected values for hyperbolic eccentric anomaly. These values were converted
    // back and forth to verify their correctness. Also the conversion was verified by comparing
    // with GTOP. GTOP uses a different definition for the hyperbolic eccentric anomaly. Hence
    // the mean anomalies were converted to cartesian positions and compared to the same conversion
    // using Tudat methods. (GTOP also does not use true anomaly as explicit step.)
    const double arrayOfExpectedHyperbolicAnomalies [ 6 ] = { 1.9132897042137,
                                                              2.60400218106873,
                                                              0.478057581067141,
                                                              -1.50971422579796,
                                                              0.529595060186511,
                                                              0.0253214157050963 };

    // Loop over sets of data.
    for ( int counter = 0; counter < 6; counter++ )
    {
        // Compute the hyperbolic eccentric anomaly.
        const double hyperbolicEccentricAnomaly = convertMeanAnomalyToHyperbolicEccentricAnomaly(
                    arrayOfTestEccentricities[ counter ], arrayOfTestMeanAnomalies[ counter ] );

        // Check if computed eccentric anomaly is less than error tolerance.
        BOOST_CHECK_CLOSE_FRACTION( arrayOfExpectedHyperbolicAnomalies[ counter ],
                                    hyperbolicEccentricAnomaly,
                                    1.0E-14 );
    }
}

//! Test 2: Test a value that is out of range.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToHyperbolicEccentricAnomaly_TooLow )
{
    // Set test value for eccentricity.
    const double testEccentricity = 0.5;

    // Set test value for mean anomaly.
    const double testMeanAnomaly = 0.5;

    // Check if a runtime error is thrown if the anomaly is converted for this eccentricity.
    BOOST_CHECK_THROW( convertMeanAnomalyToHyperbolicEccentricAnomaly(
                           testEccentricity, testMeanAnomaly ), std::runtime_error );
}

//! Test 3: Test conversion for near-parabolic orbits.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToHyperbolicEccentricAnomaly_nearParabolic )
{
    // Set test value for eccentricity.
    const double testEccentricity = 1.0 + 1.0e-10;

    // Set array of test values for mean anomaly.
    const double arrayOfTestMeanAnomalies[ 4 ] = { -10.0, -1.4, 0.5, 7.6 };

    // Set array of expected values for hyperbolic eccentric anomaly. These values were converted
    // back and forth to verify their correctness. Also the conversion was verified by comparing
    // with GTOP. GTOP uses a different definition for the hyperbolic eccentric anomaly. Hence
    // the mean anomalies were converted to cartesian positions and compared to the same conversion
    // using Tudat methods. (GTOP also does not use true anomaly as explicit step)
    const double arrayOfExpectedHyperbolicAnomalies [ 4 ] = { -3.280887528670698,
                                                              -1.913052492643601,
                                                              1.396250871565077,
                                                              3.062027761338891 };

    // Loop over sets of data.
    for ( int counter = 0; counter < 4; counter++ )
    {
        // Compute the hyperbolic eccentric anomaly.
        const double hyperbolicEccentricAnomaly = convertMeanAnomalyToHyperbolicEccentricAnomaly(
                    testEccentricity, arrayOfTestMeanAnomalies[ counter ] );

        // Check if computed eccentric anomaly is less than error tolerance.
        BOOST_CHECK_CLOSE_FRACTION( arrayOfExpectedHyperbolicAnomalies[ counter ],
                                    hyperbolicEccentricAnomaly,
                                    1.0E-14 );
    }

}

//! Generalized function to test many to and fro mean to hyperbolic eccentric anomalies.
/*!
 *  Generalized function to test many to and fro mean to hyperbolic eccentric anomalies.
 *  The function allows random variations of both the mean anomlay and eccentricity. The
 *  eccentricity may be fixed, for instance to test near-parabolic orbits for many mean
 *  anomalies. Furthermore, the range of mean anomalies and eccentricities may be set
 *  as exponential, so that a random number of 4.5 generates an eccentricity of 10^(4.5), to allow
 *  testing of extreme orbits.
 *  \param caseId Name of test case, to be used for error output purposes.
 *  \param testTolerance Acceptance tolerance to be used for difference between original and
 *  reconverted mean anomaly. Tolerance is absolute if useExponentialValues is false and relative
 *  if useExponentialValues is true.
 *  \param meanAnomalyLimit Limit of mean anomaly values, range of mean anomalies is set as
 *  [-meanAnomalyLimit,meanAnomalyLimit].
 *  \param useConstantEccentricity Boolean determining if a constant eccentricity is used.
 *  \param useExponentialValues Boolean determining whether exponential values are used for
 *  random mean anomalies and eccentricities.
 *  \param minimumEccentricity Minimum value to be used for random eccentricities (only used if
 *  useConstantEccentricity is false).
 *  \param maximumEccentricity Maximum value to be used for random eccentricities (only used if
 *  useConstantEccentricity is false).
 *  \param constantEccentricity Constant value to be used for eccentricity (only used if
 *  useConstantEccentricity is true).
 *  \param numberOfSamples Number of random cases that are to be tested.
 */
template< typename ScalarType >
void testMeanToHyperbolicEccentricAnomalyConversions(
        const std::string& caseId,
        const ScalarType testTolerance,
        const ScalarType meanAnomalyLimit,
        const bool useConstantEccentricity = 0,
        const bool useExponentialValues = 0,
        const ScalarType minimumEccentricity = TUDAT_NAN,
        const ScalarType maximumEccentricity = TUDAT_NAN,
        const ScalarType constantEccentricity = TUDAT_NAN,
        const int numberOfSamples = 1E5)
{
    // Create vectors that will store the input variables of a test that resulted in an error, such
    // that the error scenario can be reproduced.
    std::vector< double > failedMeanAnomalies, failedEccentricities;

    // Boolean that will be set true if a runtime error occurred.
    bool aRuntimeErrorOccurred = false;

    // Set test value for eccentricity.
    ScalarType testEccentricity = constantEccentricity;

    // Initialize both test and reverse calculated mean anomaly and the eccentric anomaly.
    ScalarType testMeanAnomaly, reverseCalculatedMeanAnomaly, eccentricAnomaly = 0.0;

    // Instantiate random number generator.
    boost::mt19937 randomNumbergenerator( time( 0 ) );

    // Create generator for eccentricity (only used if useConstantEccentricity is false).
    boost::random::uniform_real_distribution< > eccentricityDistribution;
    if( !useConstantEccentricity )
    {
        eccentricityDistribution =
                boost::random::uniform_real_distribution< >(
                    minimumEccentricity, maximumEccentricity );
    }

    boost::variate_generator< boost::mt19937&, boost::random::uniform_real_distribution < > >
            eccentricityGenerator(
                randomNumbergenerator, eccentricityDistribution );

    // Create generator for mean anomaly.
    boost::random::uniform_real_distribution< ScalarType > meanAnomalyDistibution(
                -meanAnomalyLimit, meanAnomalyLimit );
    boost::variate_generator<
            boost::mt19937&, boost::random::uniform_real_distribution < ScalarType > >
            generateMeanAnomaly( randomNumbergenerator, meanAnomalyDistibution );

    // Perform the conversion for the specified number of samples and test whether the values that
    // are subsequently converted back match the initial values.
    for ( int counter = 0; counter < numberOfSamples; counter++ )
    {
        // Set random value in test mean anomaly.
        testMeanAnomaly = generateMeanAnomaly( );

        if( useExponentialValues )
        {
            testMeanAnomaly = testMeanAnomaly *
                    std::pow( getFloatingInteger< ScalarType >( 10 ), generateMeanAnomaly( ) );
        }
        // If eccentricity is to be varied, generate random value
        if( !useConstantEccentricity )
        {
            testEccentricity = eccentricityGenerator( );

            if( useExponentialValues )
            {
                testEccentricity = getFloatingInteger< ScalarType >( 1 ) +
                        std::pow( getFloatingInteger< ScalarType >( 10 ), testEccentricity );
            }
        }

        // If the Rootfinder does not converge, it will produce a runtime error. In order to make
        // sure that these values that led to the error will not be lost, they will be stored in
        // the failed input data vectors. To do so, a try-catch sequence is used.
        try
        {
            // Compute eccentric anomaly.
            eccentricAnomaly = convertMeanAnomalyToHyperbolicEccentricAnomaly< ScalarType>(
                        testEccentricity, testMeanAnomaly );
        }
        catch( std::runtime_error )
        {
            // Store the fact that a runtime error occurred, such that the values will be stored.
            aRuntimeErrorOccurred = true;
        }

        // Calculate the mean anomaly from this eccentric anomaly.
        reverseCalculatedMeanAnomaly = convertHyperbolicEccentricAnomalyToMeanAnomaly< ScalarType>(
                    eccentricAnomaly, testEccentricity );

        // Test whether the computed mean anomaly is equal to the mean anomaly from the input and
        // that no runtime errors occurred. If an error was found, store the values leading to this
        // error in a vector for later use. '!' operator is there to ensure that a NaN value will
        // result in the values being written away. It is also checked that the mean anomaly is
        // not equal to 0.0, because that would result in falsely writing an error.
        if( !useExponentialValues )
        {
            if ( ( ( !( std::abs( testMeanAnomaly - reverseCalculatedMeanAnomaly ) <
                        testTolerance ) )
                   && !( testMeanAnomaly == getFloatingInteger< ScalarType >( 0 ) ||
                         reverseCalculatedMeanAnomaly == getFloatingInteger< ScalarType >( 0 ) ) )
                 && !aRuntimeErrorOccurred )
            {
                failedMeanAnomalies.push_back( testMeanAnomaly );
                failedEccentricities.push_back( testEccentricity );
            }
        }
        else
        {
            if ( ( ( !( std::abs( testMeanAnomaly - reverseCalculatedMeanAnomaly ) /
                        testMeanAnomaly < testTolerance ) )
                   && !( testMeanAnomaly == getFloatingInteger< ScalarType >( 0 ) ||
                         reverseCalculatedMeanAnomaly == getFloatingInteger< ScalarType >( 0 ) ) )
                 && !aRuntimeErrorOccurred )
            {
                failedMeanAnomalies.push_back( testMeanAnomaly );
                failedEccentricities.push_back( testEccentricity );
            }
        }

        // Reset boolean.
        aRuntimeErrorOccurred = false;
    }

    // Check that no values have been written to the failedMeanAnomalies vector.  If so, this test
    // is passed. Otherwisely these values will be written away and this test will fail.
    BOOST_CHECK( failedMeanAnomalies.empty( ) );

    // If the vector is not empty, write the failed cases of this test case to a file.
    if ( !( failedMeanAnomalies.empty( ) ) )
    {
        writeErrorsToFile( failedEccentricities, failedMeanAnomalies, caseId );
    }
}

//! Test 4: Test large number of anomalies and eccentricities
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToEccentricAnomaly_nearParabolic_random_double )
{
    long double ratioOfPrecision = std::numeric_limits< long double >::epsilon( ) /
            std::numeric_limits< double >::epsilon( );

    // Test random conversions for near-parabolic orbits
    testMeanToHyperbolicEccentricAnomalyConversions< double >(
                "DoubleParabolic", 1.0E-13, 20.0, 1, 0, TUDAT_NAN, TUDAT_NAN, 1.0 + 1.0e-15 );
    testMeanToHyperbolicEccentricAnomalyConversions< long double >(
                "LongDoubleParabolic", 1.0E-13L * ratioOfPrecision, 20.0L, 1, 0,
                TUDAT_NAN, TUDAT_NAN, 1.0L + 1.0e-15L * ratioOfPrecision );

    // Test random conversions for random orbits
    testMeanToHyperbolicEccentricAnomalyConversions< double >(
                "DoubleTypical", 1.0E-13, 20.0, 0, 0, 1.0 + 1.0e-15, 10.0, TUDAT_NAN );
    testMeanToHyperbolicEccentricAnomalyConversions< long double >(
                "LongDoubleTypical", 1.0E-13L, 20.0L, 0, 0, 1.0L + 1.0e-15L * ratioOfPrecision,
                10.0L, TUDAT_NAN );

    // Test random conversions for extreme values of eccentricity and mean anomaly.
    testMeanToHyperbolicEccentricAnomalyConversions< double >(
                "DoubleHighlyEccentric", 1.0E-14, 12.0, 0, 1, 0.0, 15.0, TUDAT_NAN );
    testMeanToHyperbolicEccentricAnomalyConversions< long double >(
                "LongDoubleHighlyEccentric", 1.0E-14L, 12.0L, 0, 1, 0.0L, 15.0L, TUDAT_NAN );

}

//! Test 5: Test functionality of specifying the initial guess.
BOOST_AUTO_TEST_CASE( test_convertMeanAnomalyToHyperbolicEccentricAnomaly_specificInitialGuess )
{
    // Set test value for eccentricity.
    const double testEccentricity = 1.97;

    // Set test value for mean anomaly.
    const double testHyperbolicMeanAnomaly = 0.5;

    // Set expected hyperbolic eccentric anomaly. (Similar case as in Test 1.)
    const double expectedHyperbolicEccentricAnomaly = 0.478057581067141;

    // Set the initial guess.
    const double initialGuess = 2.0 * testHyperbolicMeanAnomaly / testEccentricity - 1.8;

    // Compute eccentric anomaly.
    const double hyperbolicEccentricAnomaly = convertMeanAnomalyToHyperbolicEccentricAnomaly(
                testEccentricity, testHyperbolicMeanAnomaly, false, initialGuess );

    // Check if computed eccentric anomaly is NaN for invalid eccentricity.
    BOOST_CHECK_CLOSE_FRACTION( expectedHyperbolicEccentricAnomaly, hyperbolicEccentricAnomaly,
                                1.0E-14 );
}

// End Boost test suite.
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef UTILS_HPP
#define UTILS_HPP

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

template <typename qp_t>
void print_qp(qp_t qp)
{
    Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ",", "[", "]", "[", "]");
    std::cout << "P = " << qp.P.format(fmt) << '\n';
    std::cout << "q = " << qp.q.transpose().format(fmt) << '\n';
    std::cout << "A = " << qp.A.format(fmt) << '\n';
    std::cout << "l = " << qp.l.transpose().format(fmt) << '\n';
    std::cout << "u = " << qp.u.transpose().format(fmt) << '\n';
}

template <typename Mat>
bool is_psd(Mat &h)
{
    Eigen::EigenSolver<Mat> eigensolver(h);
    for (int i = 0; i < eigensolver.eigenvalues().RowsAtCompileTime; i++) {
        double v = eigensolver.eigenvalues()(i).real();
        if (v <= 0) {
            return false;
        }
    }
    return true;
}

#endif /* UTILS_HPP */


/* boost random/lognormal_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: lognormal_distribution.hpp,v 1.1.1.1 2007/10/29 07:32:44 cvsadmin Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LOGNORMAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_LOGNORMAL_DISTRIBUTION_HPP

#include <cmath>      // std::exp, std::sqrt
#include <cassert>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/normal_distribution.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std {
  using ::log;
  using ::sqrt;
}
#endif

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::sqrt;
  using std::exp;
#endif

template<class RealType = double>
class lognormal_distribution
{
public:
  typedef typename normal_distribution<RealType>::input_type input_type;
  typedef RealType result_type;

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit lognormal_distribution(result_type mean = result_type(1),
                                  result_type sigma = result_type(1))
    : _mean(mean), _sigma(sigma)
  {
    assert(mean > result_type(0));
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine

  RealType& mean() const { return _mean; }
  RealType& sigma() const { return _sigma; }
  void reset() { _normal.reset(); }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::exp;
#endif
    return exp(_normal(eng) * _nsigma + _nmean);
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const lognormal_distribution& ld)
  {
    os << ld._normal << " " << ld._mean << " " << ld._sigma;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, lognormal_distribution& ld)
  {
    is >> std::ws >> ld._normal >> std::ws >> ld._mean >> std::ws >> ld._sigma;
    ld.init();
    return is;
  }
#endif

private:
  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::exp; using std::log; using std::sqrt;
#endif
    _nmean = log(_mean*_mean/sqrt(_sigma*_sigma + _mean*_mean));
    _nsigma = sqrt(log(_sigma*_sigma/_mean/_mean+result_type(1)));
  }

  RealType _mean, _sigma;
  RealType _nmean, _nsigma;
  normal_distribution<result_type> _normal;
};

} // namespace boost

#endif // BOOST_RANDOM_LOGNORMAL_DISTRIBUTION_HPP


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

#ifndef DCS_MATH_STATS_DISTRIBUTION_WEIBULL_HPP
#define DCS_MATH_STATS_DISTRIBUTION_WEIBULL_HPP

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

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

#include <boost/math/distributions/weibull.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The Weibull distribution with shape parameter \f$k\f$ and scale
 *  parameter \f$\lambda\f$.
 *
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|k,\lambda)=\begin{cases} \frac{k}{\lambda}\left(\frac{x}{\lambda}\right)^{k-1}e^{-(x/\lambda)^{k}} & x\geq0\\ 0 & x<0\end{cases}
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class weibull_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit weibull_distribution(support_type shape, support_type scale=1)
		: dist_(shape, scale)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * weibull distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this weibull
	 * distribution.
	 *
	 * A \c weibull random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|k,\lambda)=\begin{cases} \frac{k}{\lambda}\left(\frac{x}{\lambda}\right)^{k-1}e^{-(x/\lambda)^{k}} & x\geq0\\ 0 & x<0\end{cases}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&, support_type> eng(rng);

		return dist_.scale()*(::std::pow(-::std::log(eng()),value_type(1)/dist_.shape()));
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * weibull distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * weibull distribution.
	 *
	 * A \c weibull random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\lambda) = \lambda e^{-\lambda x}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n)
	{
		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(rand(rng));
		}

		return rnds;
	}

	public: support_type shape() const
	{
		return dist_.shape();
	}

	public: support_type scale() const
	{
		return dist_.scale();
	}

	public: support_type location() const
	{
		return support_type(0);
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	private: ::boost::math::weibull_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, weibull_distribution<RealT,PolicyT> const& dist)
{
	return os << "Weibull("
			  << "shape=" <<  dist.shape()
			  << ", scale=" <<  dist.scale()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_WEIBULL_HPP


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

#include "sys.h"
#include <time.h>
#include "grid.h"
#include <armadillo>

using namespace std;

double compute_convolution_normal(System *sys, double *pNoise, Node *C) {
    double sum = 0;

    for (int i = 1; i < 2 * sys->len; i++) {
        int samp_y = (C[i].x + C[i - 1].x) * 0.5;
        int samp_x = (C[i].y + C[i - 1].y) * 0.5;

        if (samp_x < 0)
            samp_x = 0;
        if (samp_y < 0)
            samp_y = 0;

        if (samp_x > sys->NGrid)
            samp_x = sys->NGrid - 1;
        if (samp_y > sys->NGrid)
            samp_y = sys->NGrid - 1;

        int pic_loc = samp_y * sys->NGrid + samp_x;

        sum = sum + pNoise[pic_loc];
    }

    sum = sum / (2 * sys->len + 1);

    return sum;
}

double compute_convolution_high(System *sys, double *pNoise, Node *C) {
    double sum = 0;

    double filter[3][3] = {
        0, 1 / 2, 0,
        1 / 2, 1.5, 1 / 2,
        0, 1 / 2, 0
    };

    for (int i = 1; i < 2 * sys->len; i++) {
        int samp_x = (C[i].x + C[i - 1].x) * 0.5;
        int samp_y = (C[i].y + C[i - 1].y) * 0.5;

        if (samp_x < 0)
            samp_x = 0;
        if (samp_y < 0)
            samp_y = 0;

        if (samp_x > sys->NGrid)
            samp_x = sys->NGrid - 1;
        if (samp_y > sys->NGrid)
            samp_y = sys->NGrid - 1;

        for (int r = 0; r < 3; r++) {
            for (int s = 0; s < 3; s++) {
                int n = (samp_x - 3 / 2 + r + sys->NGrid) % sys->NGrid;
                int m = (samp_y - 3 / 2 + s + sys->NGrid) % sys->NGrid;
                int pic_loc = n * sys->NGrid + m;
                sum = sum + (pNoise[pic_loc] * filter[r][s]);
            }
        }
    }

    sum = sum / (2 * sys->len + 1);

    return sum;
}

void synthesize_vectors(System *sys, Grid *grid, double **vect_x, double **vect_y) {
    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            vect_x[j][i] = grid->Hy(j,i);
            vect_y[j][i] = grid->Hz(j,i);
        }
    }
}

void normalize_vectors(System *sys, double **vect_x, double **vect_y) {
    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            double mag = sqrtf(vect_x[j][i] * vect_x[j][i] + vect_y[j][i] * vect_y[j][i]);

            if (mag > 0.0) {
                vect_x[j][i] = vect_x[j][i] / mag;
                vect_y[j][i] = vect_y[j][i] / mag;
            }
            else {
                vect_x[j][i] = 0;
                vect_y[j][i] = 0;
            }
        }
    }
}

void white_noise(System *sys, double *pNoise) {
    int pic_loc;

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            int r = rand();
            pic_loc = j * sys->NGrid + i;

            r = ((r & 255) + ((r & 255) >> 8)) & 255;
            pNoise[pic_loc] = (unsigned char) r;
        }
    }
}

void compute_integral_curve(System *sys, Node *p, Node *C, double **vect_x, double **vect_y) {
    double x = p->x + 0.5;
    double y = p->y + 0.5;

    int s;
    int index = 0;

    C[index].x = x;
    C[index].y = y;

    double segLen = 0;
    double vctr_x = 0;
    double vctr_y = 0;

    // positive calculations
    for (s = 0; s < sys->len; s++) {
        if (x < 0)
            x = 0;
        if (y < 0)
            y = 0;

        if (x > sys->NGrid)
            x = sys->NGrid - 1;
        if (y > sys->NGrid)
            y = sys->NGrid - 1;

        vctr_x = vect_x[(int) x][(int) y];
        vctr_y = vect_y[(int) x][(int) y];

        segLen += 0.1;

        x = x + segLen * vctr_x;
        y = y + segLen * vctr_y;

        C[index].x = x;
        C[index].y = y;

        index++;
    }

    x = p->x + 0.5;
    y = p->y + 0.5;

    segLen = 0;

    // negative calculations
    for (s = 0; s < sys->len; s++) {
        if (x < 0)
            x = 0;
        if (y < 0)
            y = 0;

        if (x > sys->NGrid)
            x = sys->NGrid - 1;
        if (y > sys->NGrid)
            y = sys->NGrid - 1;

        vctr_x = vect_x[(int) x][(int) y];
        vctr_y = vect_y[(int) x][(int) y];

        segLen += 0.1;

        x = x - segLen * vctr_x;
        y = y - segLen * vctr_y;

        C[index].x = x;
        C[index].y = y;

        index++;
    }
}

void lic(System *sys, Grid *grid) {
    grid->ix.reshape(sys->NGrid, sys->NGrid);
    grid->iy.reshape(sys->NGrid, sys->NGrid);
    grid->iz.reshape(sys->NGrid, sys->NGrid);

    sys->len = 20;

    std::cout << "\n--- LIC algorithm started ---" << std::endl;

    clock_t begin, end;
    double time_spent;

    begin = clock();

    Node *p = (Node *) calloc(1, sizeof(Node));
    Node *C = (Node *) calloc(2 * sys->len, sizeof(Node));

    double *pNoise = (double *) malloc(sizeof(double) * sys->NGrid * sys->NGrid);
    double *pNoise_double = (double *) malloc(sizeof(double) * sys->NGrid * sys->NGrid);

    double **vect_x = (double **) calloc(sys->NGrid, sizeof(double *));
    double **vect_y = (double **) calloc(sys->NGrid, sizeof(double *));

    for (int i = 0; i < sys->NGrid; i++)
        vect_x[i] = (double *) calloc(sys->NGrid, sizeof(double));
    for (int i = 0; i < sys->NGrid; i++)
        vect_y[i] = (double *) calloc(sys->NGrid, sizeof(double));

    synthesize_vectors(sys, grid, vect_x, vect_y);
    normalize_vectors(sys, vect_x, vect_y);
    white_noise(sys, pNoise);

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            p->x = j;
            p->y = i;

            compute_integral_curve(sys, p, C, vect_x, vect_y);
            double sum = compute_convolution_normal(sys, pNoise, C);

            if (sum < 100)
                sum = 0;
            if (sum > 150)
                sum = 255;

            pNoise_double[j * sys->NGrid + i] = sum;
        }
    }

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            p->x = j;
            p->y = i;

            compute_integral_curve(sys, p, C, vect_x, vect_y);
            double sum = compute_convolution_high(sys, pNoise_double, C);

            if (sum < 0)
                sum = 0;
            if (sum > 255)
                sum = 255;

            grid->ix(j,i) = sum;
        }
    }

    arma::mat magVect(sys->NGrid, sys->NGrid);

    for (int j = 0; j < sys->NGrid; j++) {
        for (int i = 0; i < sys->NGrid; i++) {
            double mag_x = grid->Hx(j,i) * grid->Hx(j,i);
            double mag_y = grid->Hy(j,i) * grid->Hy(j,i);
            double mag_z = grid->Hz(j,i) * grid->Hz(j,i);

            double mag = sqrtf(mag_x + mag_y + mag_z);

            if (mag > 0)
                magVect(j,i) = mag;
        }
    }

    for (int j = 0; j < sys->NGrid; j++)
        for (int i = 0; i < sys->NGrid; i++)
            grid->iy(j,i) = magVect(j,i);

    end = clock();
    time_spent = (double) (end - begin) / CLOCKS_PER_SEC;

    printf("LIC End\n");
    printf("Time Spend: %lf\n", time_spent);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <boost/config.hpp>
#ifdef BOOST_NO_CXX11_HDR_ARRAY

int main()
{
    return 0;
}

#else //this example needs C++11 std::array

//[std_array
//` Shows how to use a C++11 std::array using Boost.Geometry's distance, set and assign_values algorithms

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/adapted/std_array.hpp>

BOOST_GEOMETRY_REGISTER_STD_ARRAY_CS(cs::cartesian)

int main()
{
    std::array<float, 2> a = { {1, 2} };
    std::array<double, 2> b = { {2, 3} };
    std::cout << boost::geometry::distance(a, b) << std::endl;

    boost::geometry::set<0>(a, 1.1f);
    boost::geometry::set<1>(a, 2.2f);
    std::cout << boost::geometry::distance(a, b) << std::endl;

    boost::geometry::assign_values(b, 2.2, 3.3);
    std::cout << boost::geometry::distance(a, b) << std::endl;

    boost::geometry::model::linestring<std::array<double, 2> > line;
    line.push_back(b);

    return 0;
}

//]

#endif //BOOST_NO_CXX11_HDR_ARRAY

//[std_array_output
/*`
Output:
[pre
1.41421
1.20416
1.55563
]
*/
//]


/*
The MIT License

Copyright (c) 2015-2017 Albert Murienne

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.
*/

#include "edge_detect.h"

#include <boost/type_traits/is_same.hpp>
#include <boost/math/special_functions/pow.hpp>

#include <iostream>

using namespace cimg_library;

extern "C" {
	#include "ccv.h"
}

////////////////////////////////////// SOBEL CCV /////////////////////////////////////////////////

template<> void sobel_ccv::process<unsigned char>( const CImg<unsigned char>& image_in, CImg<unsigned char>& image_out )
{
	ccv_dense_matrix_t* ccv_image_in =
		ccv_dense_matrix_new( image_in.height(), image_in.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_in.data(), 0);
	ccv_image_in->step = image_in.width() * sizeof(unsigned char);

	ccv_dense_matrix_t* ccv_image_out =
		ccv_dense_matrix_new( image_out.height(), image_out.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_out.data(), 0);
	ccv_image_out->step = image_out.width() * sizeof(unsigned char);

	ccv_sobel( ccv_image_in, &ccv_image_out, CCV_8U, 0, 1 );

	//CImg<float> test( ccv_image_out->data.f32, 50, 50, 1, 1 );
	//test.display();
}

////////////////////////////////////// CANNY CCV /////////////////////////////////////////////////

template<> void canny_ccv::process<unsigned char>( const CImg<unsigned char>& image_in, CImg<unsigned char>& image_out )
{
	// http://www.kerrywong.com/2009/05/07/canny-edge-detection-auto-thresholding/
	unsigned char mean = image_in.mean();
	unsigned char low_thresh = 0.66 * mean;
	unsigned char high_thresh = 1.33 * mean;

	ccv_dense_matrix_t* ccv_image_in =
		ccv_dense_matrix_new( image_in.height(), image_in.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_in.data(), 0);
	ccv_image_in->step = image_in.width() * sizeof(unsigned char);

	ccv_dense_matrix_t* ccv_image_out =
		ccv_dense_matrix_new( image_out.height(), image_out.width(), CCV_8U | CCV_C1 | CCV_NO_DATA_ALLOC, (void*)image_out.data(), 0);
	ccv_image_out->step = image_out.width() * sizeof(unsigned char);

	ccv_canny( ccv_image_in, &ccv_image_out, CCV_8U, 1, low_thresh, high_thresh );
}

////////////////////////////////////// SOBEL /////////////////////////////////////////////////

// use with normalized [0,1] floating point images
template<typename T>
void sobel::process( const CImg<T>& image_in, CImg<T>& image_out )
{
	//static_assert( boost::is_same<T,float>::value || boost::is_same<T,double>::value,
	//	"Template type should be floating point type!" );

	T upper_bound = 1;
	T lower_bound = 0;
	T sum;
	T sumX, sumY;

	T GX[3][3];
	T GY[3][3];

	//Sobel Matrices Horizontal
	GX[0][0] = 1; GX[0][1] = 0; GX[0][2] = -1;
	GX[1][0] = 2; GX[1][1] = 0; GX[1][2] = -2;
	GX[2][0] = 1; GX[2][1] = 0; GX[2][2] = -1;

	//Sobel Matrices Vertical
	GY[0][0] =  1; GY[0][1] =	 2; GY[0][2] =   1;
	GY[1][0] =  0; GY[1][1] =	 0; GY[1][2] =   0;
	GY[2][0] = -1; GY[2][1] =	-2;	GY[2][2] =  -1;

	/*Edge detection using Sobel Algorithm*/

	for( int y = 0; y < image_in.height() ; y++)
	{
		for( int x = 0; x < image_in.width() ; x++)
		{
			sumX	= 0;
			sumY	= 0;

			/*Image Boundaries*/
			if( y == 0 || y == image_in.height() - 1 )
				sum = 0;
			else if( x == 0 || x == image_in.width() - 1 )
				sum = 0;
			else
			{
				/*Convolution for X*/
				for( int i = -1; i < 2; i++ )
				{
					for( int j = -1; j < 2; j++ )
					{
						sumX = sumX + GX[j+1][i+1] * image_in(x+j,y+i);
					}
				}

				/*Convolution for Y*/
				for( int i = -1; i < 2; i++ )
				{
					for( int j = -1; j < 2; j++ )
					{
						sumY = sumY + GY[j+1][i+1] * image_in(x+j,y+i);
					}
				}

				/*Edge strength*/
				sum = std::sqrt( boost::math::pow<2>( sumX ) + boost::math::pow<2>( sumY ) );
			}

			if(sum > upper_bound) sum = upper_bound;
			if(sum < lower_bound) sum = lower_bound;

			image_out(x,y) = sum;//( upper_bound - sum );

			//std::cout << "x " << x << " y " << y << " SUM " << image_out(x,y) << std::endl;
		}
	}
}

template void sobel::process<float>( const CImg<float>& image_in, CImg<float>& image_out );

////////////////////////////////////// CANNY /////////////////////////////////////////////////

#define MAX_SIZE 5

//***************************
// helper function that returns true if a>b and c
//***************************
bool is_first_max( int a, int b, int c )
{
	return ( a>b && a>c );
}

//***************************
// convolve is a general helper funciton that applies a convolution
// to the image and then returns the weighted sum so that
// it can replace whatever pixel we were just analyzing
//**************************
template<typename T,const int dim>
T convolve( const CImg<T>& image_in, T con[][MAX_SIZE], T divisor, int i, int j )
{
    int midx = dim/2;
    int midy = dim/2;

	T weighted_sum = 0;
	for( int x = i-midx; x < i + dim-midx; x++ )
	{
		for( int y = j-midy; y < j + dim-midy; y++ )
		{
			weighted_sum += divisor * con[x-i+midx][y-j+midy] * image_in(x,y);
		}
	}
	return weighted_sum;
}

//*****************************
//helper function that says whether arg is between a-b or c-d
//*****************************
bool is_between( float arg, float a, float b, float c, float d )
{
	return ( ( arg >= a && arg <= b ) || ( arg >= c && arg <= d ) );
}

//****************************
// buckets the thetas into 0, 45, 90, 135
//****************************
int get_orientation( float angle )
{
	if( is_between( angle, -22.5, 22.5, -180, -157.5 ) || is_between( angle, 157.5, 180, -22.5, 0 ) )
		return 0;
	if( is_between( angle, 22.5, 67.5, -157.5, -112.5 ) )
		return 45;
	if( is_between( angle, 67.5, 112.5, -112.5, -67.5) )
		return 90;
	if( is_between( angle, 112.5, 157.5, -67.5, -22.5  ) )
		return 135;

	return -1;
}

template<typename T>
canny<T>::canny( const int& columns, const int& rows )
	: m_rows( rows ), m_columns( columns ),
	m_low_thresh( 0 ), m_high_thresh( 0 ),
	m_thetas( boost::extents[columns][rows] ), m_mag_array( boost::extents[columns][rows] )
{
	static_assert( boost::is_same<T,float>::value || boost::is_same<T,double>::value,
		"Template type should be floating point type!" );
}

template canny<float>::canny( const int& rows, const int& columns );

template<typename T>
canny<T>::~canny()
{
}

template canny<float>::~canny();

//*****************************
// gaussian blur
// applies a gaussian blur via a convolution of a gaussian
// matrix with sigma = 1.4. hard-coded in.
// future development could generate the gauss matrix on the fly
//*****************************
template<typename T>
void canny<T>::_gaussian_blur( const CImg<T>& image_in, CImg<T>& image_out )
{
	// define gauss matrix
	T gauss_array[5][5] = {	{2, 4, 5, 4, 2},
							{4, 9, 12,9, 4},
							{5, 12, 15, 12, 5},
							{4, 9, 12,9, 4},
							{2, 4, 5, 4, 2} };

	T gauss_divisor = 1.0/159.0;
	T sum = 0.0;

	for( auto j=2U; j < m_rows-2; j++ )
	{
		for( auto i=2U; i < m_columns-2; i++ )
		{
			sum = convolve<T,5>( image_in, gauss_array, gauss_divisor, i, j );
			image_out(i,j) = sum;
		}
	}
}

//****************************
// Applies a sobel filter to find the gradient direction
// and magnitude. those values are then stored in thetas and magArray
// so that info can be used later for further analysis
//****************************
template<typename T>
void canny<T>::_sobel( CImg<T>& image )
{
	T G_x, G_y, G;
	T sobel_y[5][5] = {	{-1, 0, 1,0,0},
						{-2, 0, 2,0,0},
						{-1, 0, 1,0,0},
						{0, 0, 0, 0, 0},
						{0, 0, 0, 0, 0} };

	T sobel_x[5][5] = {	{1, 2, 1, 0, 0},
						{0, 0, 0, 0, 0},
						{-1, -2, -1, 0, 0},
						{0, 0, 0, 0, 0},
						{0, 0, 0, 0, 0} };

	for ( auto j = 1U; j < m_rows-1; j++ )
	{
		for ( auto i = 1U; i < m_columns-1; i++ )
		{
			G_x = convolve<T,3>( image, sobel_x, 1, i, j );
			G_y = convolve<T,3>( image, sobel_y, 1, i, j );
			G = std::sqrt( G_x*G_x + G_y*G_y );

			m_thetas[i][j] = get_orientation( 180.0 * std::atan2( G_y, G_x ) / cimg::PI );

			m_mag_array[i][j] = G;
		}
	}
}

//*****************************
//non-maximum suppression
//depending on the orientation, pixels are either thrown away or accepted
//by checking it's neighbors
//*****************************
template<typename T>
void canny<T>::_no_max( CImg<T>& image )
{
	for( auto j=1U ; j < m_rows-1 ; j++ )
	{
	    for( auto i=1U ; i < m_columns-1 ; i++ )
		{
			 	//std::cout << m_thetas[i][j] << std::endl;

				switch( m_thetas[i][j] )
				{
				case 0:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i+1][j], m_mag_array[i-1][j] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				case 45:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i+1][j+1], m_mag_array[i-1][j-1] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				case 90:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i][j+1], m_mag_array[i][j-1] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				case 135:
					if( is_first_max( m_mag_array[i][j], m_mag_array[i+1][j-1], m_mag_array[i-1][j+1] ) )
					{
						image(i,j) = 1; // white
					}
					else
					{
						image(i,j) = 0; // black
					}
				break;

				default:
				break;
			}
		}
	}
}

//*******************************
//hysteresis noise filter makes lines continuous and filters out the noise
// see the pdf that we used to understand this step in english (Step 5)
//*******************************
template<typename T>
void canny<T>::_hysteresis( CImg<T>& image )
{
	bool greater_found;
	bool between_found;

	for( auto j=2U ; j < m_rows-2 ; j++ )
	{
		for( auto i=2U ; i < m_columns-2 ; i++ )
		{
			if( m_mag_array[i][j] < m_low_thresh )
			{
				image(i,j) = 0; // black
			}

			if( m_mag_array[i][j] > m_high_thresh )
			{
				image(i,j) = 1; // white
			}

			/*If pixel (x, y) has gradient magnitude between tlow and thigh and
			any of its neighbors in a 3 × 3 region around
			it have gradient magnitudes greater than thigh, keep the edge*/

			if( m_mag_array[i][j] >= m_low_thresh && m_mag_array[i][j] <= m_high_thresh)
			{
				greater_found = false;
				between_found = false;
				for( int m = -1; m < 2; m++ )
				{
					for( int n = -1; n < 2; n++ )
					{
						if( m_mag_array[i+m][j+n] > m_high_thresh )
						{
							image(i,j) = 0;
							greater_found = true;
				 		}
				 		if( m_mag_array[i][j] > m_low_thresh && m_mag_array[i][j] < m_high_thresh )
							between_found = true;
					}
				}

				if( !greater_found && between_found )
				{
					for( int m = -2; m < 3; m++ )
					{
						for( int n = -2; n < 3; n++ )
						{
							if( m_mag_array[i+m][j+n] > m_high_thresh )
								greater_found = true;
						}
					}
				}

				if( greater_found )
					image(i,j) = 0;
				else
					image(i,j) = 1;
			}
		}
	}
}

/*If pixel (x, y) has gradient magnitude between tlow and thigh and any of its neighbors in a 3 × 3 region around
it have gradient magnitudes greater than thigh, keep the edge (write out white).
If none of pixel (x, y)s neighbors have high gradient magnitudes but at least one falls between tlow and thigh,
search the 5 × 5 region to see if any of these pixels have a magnitude greater than thigh. If so, keep the edge
(write out white).
*/

template<typename T>
void canny<T>::process( const CImg<T>& image_in, CImg<T>& image_out )
{
	// http://www.kerrywong.com/2009/05/07/canny-edge-detection-auto-thresholding/
	T mean = image_in.mean();
	m_low_thresh = 0.66 * mean;
	m_high_thresh = 1.33 * mean;

	_gaussian_blur( image_in, image_out );
	_sobel( image_out );
	_no_max( image_out );
	_hysteresis( image_out );

	// 2px border not managed for now
	cimg_for_borderXY( image_out, x, y, 3 ) { image_out( x, y ) = 0; }
}

template void canny<float>::process( const CImg<float>& image_in, CImg<float>& image_out );


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2014 Erik Erlandson
//
// 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)

//#include <boost/units/systems/information.hpp>

/**
\file

\brief information.cpp

\details
Demonstrate information unit system.

Output:
@verbatim
bytes= 1.25e+08 B
bits= 8e+06 b
nats= 4605.17 nat
1024 bytes in a kibi-byte
8.38861e+06 bits in a mebi-byte
0.000434294 hartleys in a milli-nat
entropy in bits= 1 b
entropy in nats= 0.693147 nat
entropy in hartleys= 0.30103 Hart
entropy in shannons= 1 Sh
entropy in bytes= 0.125 B
@endverbatim
**/

#include <cmath>
#include <iostream>
using std::cout;
using std::endl;

#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/conversion.hpp>
namespace bu = boost::units;
using bu::quantity;
using bu::conversion_factor;

// SI prefixes
#include <boost/units/systems/si/prefixes.hpp>
namespace si = boost::units::si;

// information unit system
#include <boost/units/systems/information.hpp>
using namespace bu::information;

// Define a function for the entropy of a bernoulli trial.
// The formula is computed using natural log, so the units are in nats.
// The user provides the desired return unit, the only restriction being that it
// must be a unit of information.  Conversion to the requested return unit is
// accomplished automatically by the boost::units library.
template <typename Sys>
quantity<bu::unit<bu::information_dimension, Sys> >
bernoulli_entropy(double p, const bu::unit<bu::information_dimension, Sys>&) {
    typedef bu::unit<bu::information_dimension, Sys> requested_unit;
    return quantity<requested_unit>((-(p*log(p) + (1-p)*log(1-p)))*nats);
}

int main(int argc, char** argv) {
    // a quantity of information (default in units of bytes)
    quantity<info> nbytes(1 * si::giga * bit);
    cout << "bytes= " << nbytes << endl;

    // a quantity of information, stored as bits
    quantity<hu::bit::info> nbits(1 * si::mega * byte);
    cout << "bits= " << nbits << endl;

    // a quantity of information, stored as nats
    quantity<hu::nat::info> nnats(2 * si::kilo * hartleys);
    cout << "nats= " << nnats << endl;

    // how many bytes are in a kibi-byte?
    cout << conversion_factor(kibi * byte, byte) << " bytes in a kibi-byte" << endl;

    // how many bits are in a mebi-byte?
    cout << conversion_factor(mebi * byte, bit) << " bits in a mebi-byte" << endl;

    // how many hartleys are in a milli-nat?
    cout << conversion_factor(si::milli * nat, hartley) << " hartleys in a milli-nat" << endl;

    // compute the entropy of a fair coin flip, in various units of information:
    cout << "entropy in bits= " << bernoulli_entropy(0.5, bits) << endl;
    cout << "entropy in nats= " << bernoulli_entropy(0.5, nats) << endl;
    cout << "entropy in hartleys= " << bernoulli_entropy(0.5, hartleys) << endl;
    cout << "entropy in shannons= " << bernoulli_entropy(0.5, shannons) << endl;
    cout << "entropy in bytes= " << bernoulli_entropy(0.5, bytes) << endl;

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[convert
//` Shows how to convert a geometry into another geometry

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point;
    typedef boost::geometry::model::box<point> box;
    typedef boost::geometry::model::polygon<point> polygon;

    point p1(1, 1);
    box bx = boost::geometry::make<box>(1, 1, 2, 2);

    // Assign a box to a polygon (conversion box->poly)
    polygon poly;
    boost::geometry::convert(bx, poly);

    // Convert a point to another point type (conversion of point-type)
    boost::tuple<double, double> p2;
    boost::geometry::convert(p1, p2); // source -> target

    using boost::geometry::dsv;
    std::cout
        << "box: " << dsv(bx) << std::endl
        << "polygon: " << dsv(poly) << std::endl
        << "point: " << dsv(p1) << std::endl
        << "point tuples: " << dsv(p2) << std::endl
        ;

    return 0;
}

//]

//[convert_output
/*`
Output:
[pre
box: ((1, 1), (2, 2))
polygon: (((1, 1), (1, 2), (2, 2), (2, 1), (1, 1)))
point: (1, 1)
point tuples: (1, 1)
]
*/
//]


// example_policy_handling.cpp

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

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

// See error_handling_example.cpp for use of
// macro definition to change policy for
// domain_error - negative degrees of freedom argument
// for student's t distribution CDF,
// and catching the exception.

// See error_handling_policies.cpp for more examples.

// Boost
#include <boost/math/distributions/students_t.hpp>
using boost::math::students_t_distribution;  // Probability of students_t(df, t).
using boost::math::students_t;  // Probability of students_t(df, t) convenience typedef for double.

using boost::math::policies::policy;
using boost::math::policies::domain_error;
using boost::math::policies::ignore_error;

// std
#include <iostream>
   using std::cout;
   using std::endl;

#include <stdexcept>

// Define a (bad?) policy to ignore domain errors ('bad' arguments):
typedef policy<
      domain_error<ignore_error>
      > my_policy;

// Define my_students_t distribution with this different domain error policy:
typedef students_t_distribution<double, my_policy> my_students_t;

int main()
{  // Example of error handling of bad argument(s) to a distribution.
  cout << "Example error handling using Student's t function. " << endl;

  double degrees_of_freedom = -1; double t = -1.; // Two 'bad' arguments!

  try
  {
    cout << "Probability of ignore_error Student's t is "
      << cdf(my_students_t(degrees_of_freedom), t) << endl;
    cout << "Probability of default error policy Student's t is " << endl;
    // By contrast the students_t distribution default domain error policy is to throw,
    cout << cdf(students_t(-1), -1) << endl;  // so this will throw.
/*`
    Message from thrown exception was:
   Error in function boost::math::students_t_distribution<double>::students_t_distribution:
   Degrees of freedom argument is -1, but must be > 0 !
*/

    // We could also define a 'custom' distribution
    // with an "ignore overflow error policy" in a single statement:
    using boost::math::policies::overflow_error;
    students_t_distribution<double, policy<overflow_error<ignore_error> > > students_t_no_throw(-1);

  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }

  return 0;
} // int main()

/*

Output:

   error_policy_example.cpp
  Generating code
  Finished generating code
  error_policy_example.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\error_policy_example.exe
  Example error handling using Student's t function.
  Probability of ignore_error Student's t is 1.#QNAN
  Probability of default error policy Student's t is

  Message from thrown exception was:
     Error in function boost::math::students_t_distribution<double>::students_t_distribution: Degrees of freedom argument is -1, but must be > 0 !

*/


//==================================================================================================
/*!
  @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_REFINE_REC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REFINE_REC_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing refine_rec capabilities

    Performs a Newton-Raphson step to improve precision of reciprocate estimate.
    This function can be used in conjunction with raw_(rec)
    to add more precision to the estimate if their default
    precision is not enough.

    @par semantic:
    For any given value @c x, @c est  of floating type T:

    @code
    T r =refine_rec(x, est);
    @endcode

    is similar to

    @code
    T r = fma(fnms(est, a0, One<T>()), est, est);
    @endcode

    @see rec

  **/
  Value refine_rec(Value const & v0, Value const& est);
} }
#endif

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

#endif


#include "utils/data_generator.cpp"
#include "../src/numerical/gradient_descent/penalized_gd_solver.h"
#include "../src/logging/easylogging++.h"
#include <armadillo>

using arma::mat;

INITIALIZE_EASYLOGGINGPP

mat WEIGHTS = {100, -20, 50, -0.5};

int main(int argc, char *argv[])
{
    el::Configurations conf("./logging-config.conf");
    el::Loggers::reconfigureLogger("default", conf);

    auto data_generator = DataGenerator();
    auto L = data_generator.generate_library();
    auto s = data_generator.generate_signal(WEIGHTS);

    LOG(INFO) << "True: " << WEIGHTS;

    PenalizedGDSolver solver = PenalizedGDSolver(L, 100, 500, 0.0000000001, 1000000);
    mat result = solver.solve(s);
    LOG(INFO) << "Penalized GD solver fit: " << result;

    return 0;
}