//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/math/generate_binary_vectors.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/object_impl.hpp>
#include <fcppt/math/vector/output.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <array>
#include <iostream>
#include <iterator>
#include <ostream>
#include <vector>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_binary_vectors
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::vector::static_<
		int,
		2
	>
	vector2;

	typedef
	fcppt::math::vector::static_<
		int,
		3
	>
	vector3;

	typedef
	std::array<
		vector2,
		4
	>
	vector2_container;

	typedef
	std::array<
		vector3,
		8
	>
	vector3_container;

	std::cout << "Generating 2D binary vectors...\n";

	vector2_container const result2(
		fcppt::math::generate_binary_vectors<
			int,
			2
		>()
	);

	std::cout << "Result: \n";

	for(
		auto const &element
		:
		result2
	)
		std::cout << element << '\n';

	std::cout << "Now checking...\n";

	BOOST_REQUIRE_EQUAL(
		result2.size(),
		4u
	);

	BOOST_CHECK_EQUAL(
		result2[0],
		vector2(0,0)
	);

	BOOST_CHECK_EQUAL(
		result2[1],
		vector2(1,0)
	);

	BOOST_CHECK_EQUAL(
		result2[2],
		vector2(0,1)
	);

	BOOST_CHECK_EQUAL(
		result2[3],
		vector2(1,1)
	);

	std::cout << "Generating 3D binary vectors...\n";

	vector3_container const result3(
		fcppt::math::generate_binary_vectors<
			int,
			3
		>()
	);

	std::cout << "Result: \n";

	for(
		auto const &element
		:
		result3
	)
		std::cout << element << '\n';

	std::cout << "Now checking...\n";

	BOOST_REQUIRE_EQUAL(
		result3.size(),
		8u
	);

	BOOST_CHECK_EQUAL(
		result3[0],
		vector3(0,0,0)
	);

	BOOST_CHECK_EQUAL(
		result3[1],
		vector3(1,0,0)
	);

	BOOST_CHECK_EQUAL(
		result3[2],
		vector3(0,1,0)
	);

	BOOST_CHECK_EQUAL(
		result3[3],
		vector3(1,1,0)
	);

	BOOST_CHECK_EQUAL(
		result3[4],
		vector3(0,0,1)
	);

	BOOST_CHECK_EQUAL(
		result3[5],
		vector3(1,0,1)
	);

	BOOST_CHECK_EQUAL(
		result3[6],
		vector3(0,1,1)
	);

	BOOST_CHECK_EQUAL(
		result3[7],
		vector3(1,1,1)
	);
}


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef QR_HPP
#define QR_HPP

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

// QR decomposition using Gram-Schmit orthogonalization

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> bool qr(const ub::matrix<T>& in, ub::matrix<T>& q, ub::matrix<T>& r)
{
	int i, j, k;
	int n;
	ub::vector<T> v;
	T tmp;

	n = in.size1();
	if (n != in.size2()) return false;

	q.resize(n, n);
	r.resize(n, n);
	v.resize(n);

	for (i=0; i<n; i++) {
		for (k=0; k<n; k++) v(k) = in(k, i);
		for (j=0; j<i; j++) {
			tmp = 0.;
			for (k=0; k<n; k++) {
				tmp += in(k, i) * q(k, j);
			}
			for (k=0; k<n; k++) {
				v(k) -= tmp * q(k, j);
			}
			r(j, i) = tmp;
		}
		tmp = norm_2(v);
		if (tmp == 0.) return false;
		for (k=0; k<n; k++) q(k, i) = v(k) / tmp;
		r(i, i) = tmp;
		for (j=i+1; j<n; j++) r(j, i) = 0.;
	}

	return true;
}

} // namespace kv

#endif // QR_HPP


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core_math

#ifndef openOR_core_Math_detail_inverse_hpp
#define openOR_core_Math_detail_inverse_hpp

#include <boost/numeric/ublas/lu.hpp>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/void.hpp>
#include <boost/type_traits/is_same.hpp>

#include <openOR/Log/Logger.hpp>
#include <openOR/Math/matrix.hpp>

#include <openOR/Math/determinant.hpp>

namespace openOR {
	namespace Math {

		struct no_inverse_matrix_error : public std::runtime_error {
			no_inverse_matrix_error() : std::runtime_error("Could not get matrix inverse") {
				LOG(Log::Level::Debug, Log::noAttribs, Log::msg("Exception thrown: Could not get matrix inverse") );
			}
		};

		namespace Impl {

			template <class Type, int Dim>
			struct Inverse {
				static Type get(const Type& mat) {
					// function has to be specialized
					BOOST_MPL_ASSERT((boost::is_same<Type, boost::mpl::void_>));
				}
			};

			template <class Type>
			struct Inverse<Type, 2> {
				static Type get(const Type& mat) {
					Type matResult;
					Math::get<0, 0>(matResult) = Math::get<1, 1>(mat);
					Math::get<0, 1>(matResult) = -Math::get<0, 1>(mat);
					Math::get<1, 0>(matResult) = -Math::get<1, 0>(mat);
					Math::get<1, 1>(matResult) = Math::get<0, 0>(mat);
					matResult /= Math::determinant(mat);
					return matResult;
				}
			};

			template <class Type>
			struct Inverse<Type, 3> {
				static Type get(const Type& mat) {
					Type matResult;
					Math::get<0, 0>(matResult) = Math::get<1, 1>(mat) * Math::get<2, 2>(mat) - Math::get<1, 2>(mat) * Math::get<2, 1>(mat);
					Math::get<0, 1>(matResult) = Math::get<0, 2>(mat) * Math::get<2, 1>(mat) - Math::get<0, 1>(mat) * Math::get<2, 2>(mat);
					Math::get<0, 2>(matResult) = Math::get<0, 1>(mat) * Math::get<1, 2>(mat) - Math::get<0, 2>(mat) * Math::get<1, 1>(mat);

					Math::get<1, 0>(matResult) = Math::get<1, 2>(mat) * Math::get<2, 0>(mat) - Math::get<1, 0>(mat) * Math::get<2, 2>(mat);
					Math::get<1, 1>(matResult) = Math::get<0, 0>(mat) * Math::get<2, 2>(mat) - Math::get<0, 2>(mat) * Math::get<2, 0>(mat);
					Math::get<1, 2>(matResult) = Math::get<0, 2>(mat) * Math::get<1, 0>(mat) - Math::get<0, 0>(mat) * Math::get<1, 2>(mat);

					Math::get<2, 0>(matResult) = Math::get<1, 0>(mat) * Math::get<2, 1>(mat) - Math::get<1, 1>(mat) * Math::get<2, 0>(mat);
					Math::get<2, 1>(matResult) = Math::get<0, 1>(mat) * Math::get<2, 0>(mat) - Math::get<0, 0>(mat) * Math::get<2, 1>(mat);
					Math::get<2, 2>(matResult) = Math::get<0, 0>(mat) * Math::get<1, 1>(mat) - Math::get<0, 1>(mat) * Math::get<1, 0>(mat);

					matResult /= Math::determinant(mat);
					return matResult;
				}
			};

			template <class Type>
			struct Inverse<Type, 4> {
				static Type get(const Type& mat) {
					Type inverse;

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

					// create a working copy of the input
					Type A(mat);

					// create a permutation matrix for the LU-factorization
					pmatrix pm(A.size1());

					// perform LU-factorization
					int res = lu_factorize(A,pm);
					if( res != 0 ) { throw no_inverse_matrix_error(); }

					// create identity matrix of "inverse"
					inverse.assign( Math::MatrixTraits<Type>::IDENTITY );

					// backsubstitute to get the inverse
					lu_substitute(A, pm, inverse);

					return inverse;
				}
			};

		}
	}
}

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Regular_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Regular_triangulation_2.h>
#include <CGAL/internal/boost/function_property_map.hpp>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <fstream>
#include <iostream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel     K;
typedef K::FT                                                   FT;
typedef K::Weighted_point_2                                     Weighted_point;

typedef CGAL::Regular_triangulation_2<K>                        Triangulation;

typedef boost::graph_traits<Triangulation>::vertex_descriptor   vertex_descriptor;
typedef boost::graph_traits<Triangulation>::vertex_iterator     vertex_iterator;
typedef boost::graph_traits<Triangulation>::edge_descriptor     edge_descriptor;

template <typename T>
struct Compute_edge_weight
{
  const T& t_;

  Compute_edge_weight(const T& t) : t_(t) { }

  typedef typename boost::graph_traits<T>::vertex_descriptor vertex_descriptor;
  typedef typename boost::graph_traits<T>::edge_descriptor   edge_descriptor;

  FT operator()(const edge_descriptor ed) const {
    vertex_descriptor svd = source(ed, t_);
    vertex_descriptor tvd = target(ed, t_);
    typename T::Vertex_handle sv = svd;
    typename T::Vertex_handle tv = tvd;
    return CGAL::power_product(sv->point(), tv->point());
  }
};

// The BGL makes use of indices associated to the vertices
// We use a std::map to store the index
typedef std::map<vertex_descriptor,int> VertexIndexMap;

// A std::map is not a property map, because it is not lightweight
typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;

int main(int argc,char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/weighted_points.xyw";
  std::ifstream input(filename);
  Triangulation tr;

  Weighted_point wp;
  while(input >> wp)
    tr.insert(wp);

  // Note that with the input "data/weighted_points.xyw", there is one hidden vertex
  std::cout << "number of hidden vertices: " << tr.number_of_hidden_vertices() << std::endl;

  // Associate indices to the vertices
  VertexIndexMap vertex_id_map;
  VertexIdPropertyMap vertex_index_pmap(vertex_id_map);
  int index = 0;

  for(vertex_descriptor vd : vertices(tr))
    vertex_id_map[vd]= index++;

  // We use a custom edge length property map that computes the power distance
  // between the extremities of an edge of the regular triangulation
  typedef Compute_edge_weight<Triangulation> Edge_weight_functor;

  // In the function call you can see a named parameter: vertex_index_map
  std::list<edge_descriptor> mst;
  boost::kruskal_minimum_spanning_tree(tr, std::back_inserter(mst),
                                       vertex_index_map(vertex_index_pmap)
                                       .weight_map(CGAL::internal::boost_::make_function_property_map<
                                          edge_descriptor, FT, Edge_weight_functor>(Edge_weight_functor(tr))));

  std::cout << "The edges of the Euclidean mimimum spanning tree:" << std::endl;
  for(edge_descriptor ed : mst)
  {
    vertex_descriptor svd = source(ed, tr);
    vertex_descriptor tvd = target(ed, tr);
    Triangulation::Vertex_handle sv = svd;
    Triangulation::Vertex_handle tv = tvd;
    std::cout << "[ " << sv->point() << "  |  " << tv->point() << " ] " << std::endl;
  }

  return EXIT_SUCCESS;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/LinearStateModel.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

void LinearStateModel::propagate(const Eigen::Ref<const Eigen::MatrixXd>& cur_states, Eigen::Ref<Eigen::MatrixXd> pred_states)
{
    pred_states = getStateTransitionMatrix() * cur_states;
}


/**
 * \file Chebyshev1Filter.hxx
 */

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/asinh.hpp>

#include <ATK/EQ/Chebyshev1Filter.h>
#include <ATK/EQ/helpers.h>

namespace Chebyshev1Utilities
{
  template<typename DataType>
  void create_chebyshev1_analog_coefficients(int order, DataType ripple, EQUtilities::ZPK<DataType>& zpk)
  {
    zpk.z.clear(); // no zeros for this filter type
    zpk.p.clear();
    if(ripple == 0)
    {
      return;
    }
    if(order == 0)
    {
      zpk.k = static_cast<DataType>(std::pow(10, (-ripple / 20)));
      return;
    }

    DataType eps = static_cast<DataType>(std::sqrt(std::pow(10, (0.1 * ripple)) - 1.0));
    DataType mu = static_cast<DataType>(1.0 / order * boost::math::asinh(1 / eps));

    for(gsl::index i = -order+1; i < order; i += 2)
    {
      DataType theta = boost::math::constants::pi<DataType>() * i / (2*order);
      zpk.p.push_back(-std::sinh(std::complex<DataType>(mu, theta)));
    }

    std::complex<DataType> f = 1;

    for(gsl::index i = 0; i < zpk.p.size(); ++i)
    {
      f *= -zpk.p[i];
    }
    zpk.k = f.real();
    if(order % 2 == 0)
    {
      zpk.k = zpk.k / std::sqrt(1 + eps * eps);
    }
  }

  template<typename DataType, typename Container>
  void create_default_chebyshev1_coeffs(size_t order, DataType ripple, DataType Wn, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order), ripple, zpk);
    EQUtilities::populate_lp_coeffs(Wn, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bp_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bp_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bs_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bs_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev1LowPassCoefficients<DataType>::Chebyshev1LowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1LowPassCoefficients<DataType_>::CoeffDataType Chebyshev1LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1LowPassCoefficients<DataType_>::CoeffDataType Chebyshev1LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1LowPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_default_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1HighPassCoefficients<DataType>::Chebyshev1HighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1HighPassCoefficients<DataType_>::CoeffDataType Chebyshev1HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1HighPassCoefficients<DataType_>::CoeffDataType Chebyshev1HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1HighPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_default_chebyshev1_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(gsl::index i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev1BandPassCoefficients<DataType>::Chebyshev1BandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev1BandPassCoefficients<DataType_>::CoeffDataType, typename Chebyshev1BandPassCoefficients<DataType_>::CoeffDataType> Chebyshev1BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1BandPassCoefficients<DataType_>::CoeffDataType Chebyshev1BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1BandPassCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_bp_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1BandStopCoefficients<DataType>::Chebyshev1BandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev1BandStopCoefficients<DataType_>::CoeffDataType, typename Chebyshev1BandStopCoefficients<DataType_>::CoeffDataType> Chebyshev1BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1BandStopCoefficients<DataType_>::CoeffDataType Chebyshev1BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1BandStopCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_bs_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }
}


#ifndef CONSTANTS_HPP_
#define CONSTANTS_HPP_

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions.hpp>
#include <complex> // constant::i
#include <limits> // constant::omega
#include <string> // constant::omega

namespace constant {

template<typename T> T alladi_grinstead() {
	T c = 0, last;

	for(std::size_t n = 1; n == 1 || c != last; ++n) {
		last = c;
		c += (boost::math::zeta<T>(n + 1) - 1) / n;
	}

	static const T ALLADI_GRINSTEAD = exp(c - 1);
	return ALLADI_GRINSTEAD;
}

template<typename T> T aperys() {
	static const T APERYS = boost::math::constants::zeta_three<T>();
	return APERYS;
}

template<typename T> T buffons() {
	static const T BUFFONS = 2 / boost::math::constants::pi<T>();
	return BUFFONS;
}

template<typename T> T catalans() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		CATALANS = 0.125 * (boost::math::trigamma<T>(0.25) - PI_SQR);
	return CATALANS;
}

template<typename T> T delian() {
	static const T DELIAN = boost::math::cbrt<T>(2);
	return DELIAN;
}

template<typename T> T e() {
	static const T E = boost::math::constants::e<T>();
	return E;
}

template<typename T> T erdos_borwein() {
	T e = 0, last;

	for(std::size_t n = 1; n == 1 || e != last; ++n) {
		last = e;
		e += 1 / (exp2(static_cast<T>(n)) - 1);
	}

	static const T ERDOS_BORWEIN = e;
	return ERDOS_BORWEIN;
}

template<typename T> T euler_mascheroni() {
	static const T EULER_MASCHERONI = boost::math::constants::euler<T>();
	return EULER_MASCHERONI;
}

template<typename T> T gauss() {
	static const T ROOT_TWO = boost::math::constants::root_two<T>(),
		PI = boost::math::constants::pi<T>(),
		GAUSS = pow(boost::math::tgamma<T>(0.25), 2)
			/ (2 * ROOT_TWO * pow(PI, static_cast<T>(1.5)));
	return GAUSS;
}

template<typename T> T gelfond_schneider() {
	static const T ROOT_TWO = boost::math::constants::root_two<T>(),
		GELFOND_SCHNEIDER = exp2(ROOT_TWO);
	return GELFOND_SCHNEIDER;
}

template<typename T> T gelfonds() {
	static const T GELFONDS = boost::math::constants::e_pow_pi<T>();
	return GELFONDS;
}

template<typename T> T giesekings() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		GIESEKINGS = (9 - boost::math::trigamma<T>(2 / static_cast<T>(3))
			+ boost::math::trigamma<T>(4 / static_cast<T>(3)))
			/ (4 * ROOT_THREE);
	return GIESEKINGS;
}

template<typename T> T glaisher_kinkelin() {
	static const T GLAISHER_KINKELIN = boost::math::constants::glaisher<T>();
	return GLAISHER_KINKELIN;
}

template<typename T> T golden_ratio() {
	static const T GOLDEN_RATIO = boost::math::constants::phi<T>();
	return GOLDEN_RATIO;
}

template<typename T> std::complex<T> i() {
	static const std::complex<T> I(0, 1);
	return I;
}

template<typename T> T inverse_golden_ratio() {
	static const T INVERSE_GOLDEN_RATIO = boost::math::constants::phi<T>() - 1;
	return INVERSE_GOLDEN_RATIO;
}

template<typename T> T khinchin() {
	static const T KHINCHIN = boost::math::constants::khinchin<T>();
	return KHINCHIN;
}

// Not Levy constant
template<typename T> T khinchin_levy() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		LN_TWO = boost::math::constants::ln_two<T>(),
		KHINCHIN_LEVY = PI_SQR / (12 * LN_TWO);
	return KHINCHIN_LEVY;
}

// Not Glaisher-Kinkelin constant
template<typename T> T kinkelin() {
	static const T GLAISHER_KINKELIN = boost::math::constants::glaisher<T>(),
		KINKELIN = 1 / static_cast<T>(12) - log(GLAISHER_KINKELIN);
	return KINKELIN;
}

template<typename T> T knuth() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		KNUTH = (1 - (1 / ROOT_THREE)) / 2;
	return KNUTH;
}

template<typename T> T levys() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		LN_TWO = boost::math::constants::ln_two<T>(),
		LEVYS = exp(PI_SQR / (12 * LN_TWO));
	return LEVYS;
}

template<typename T> T liebs() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		LIEBS = (8 * ROOT_THREE) / 9;
	return LIEBS;
}

template<typename T> T lochs() {
	static const T LN_TWO = boost::math::constants::ln_two<T>(),
		LN_TEN = boost::math::constants::ln_ten<T>(),
		PI_SQR = boost::math::constants::pi_sqr<T>(),
		LOCHS = (6 * LN_TWO * LN_TEN) / PI_SQR;
	return LOCHS;
}

template<typename T> T niven() {
	T c = 1, last;

	for(std::size_t j = 2; j == 2 || c != last; ++j) {
		last = c;
		c += 1 - 1/boost::math::zeta<T>(j);
	}

	static const T NIVEN = c;
	return NIVEN;
}

template<typename T> T nortons() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		LN_PI = log(boost::math::constants::pi<T>()),
		EULER = boost::math::constants::euler<T>(),
		LN_GLAISHER_KINKELIN = log(boost::math::constants::glaisher<T>()),
		LN_TWO = boost::math::constants::ln_two<T>(),
		NORTONS = (6 * LN_TWO
			* (24 * LN_GLAISHER_KINKELIN - 3 + 2 * EULER + LN_TWO - 2 * LN_PI)
			- PI_SQR) / PI_SQR;
	return NORTONS;
}

template<typename T> T omega() {
	T w = 0;
	std::string w_pre, w_post;

	w_pre.reserve(std::numeric_limits<T>::digits10);
	w_post.reserve(std::numeric_limits<T>::digits10);

	do {
		w_pre = static_cast<std::string>(w);
		w_pre.resize(std::numeric_limits<T>::digits10);

		const T e_w = exp(w);

		w -= ((w * e_w) - 1)
			/ (e_w * (w + 1) - ((w + 2) * (w * e_w - 1) / ((w * 2) + 2)));

		w_post = static_cast<std::string>(w);
		w_post.resize(std::numeric_limits<T>::digits10);
	} while (w_pre != w_post);

	static const T OMEGA = w;
	return OMEGA;
}

template<typename T> T one() {
	static const T ONE = 1;
	return ONE;
}

template<typename T> T pi() {
	static const T PI = boost::math::constants::pi<T>();
	return PI;
}

template<typename T> T plastic_number() {
	static const T PLASTIC_NUMBER = (boost::math::cbrt<T>(108 + 12
		* sqrt(static_cast<T>(69))) + boost::math::cbrt<T>(108 - 12
		* sqrt(static_cast<T>(69)))) / static_cast<T>(6);
	return PLASTIC_NUMBER;
}

template<typename T> T pogsons() {
	static const T POGSONS = pow(10, 2 / static_cast<T>(5));
	return POGSONS;
}

template<typename T> T polyas_random_walk() {
	static const T PI_CUBED = boost::math::constants::pi_cubed<T>(),
		ROOT_SIX = sqrt(static_cast<T>(6)),
		POLYAS_RANDOM_WALK = 1 - 1/((ROOT_SIX / (32 * PI_CUBED))
			* boost::math::tgamma<T>(1 / static_cast<T>(24))
			* boost::math::tgamma<T>(5 / static_cast<T>(24))
			* boost::math::tgamma<T>(7 / static_cast<T>(24))
			* boost::math::tgamma<T>(11 / static_cast<T>(24)));
	return POLYAS_RANDOM_WALK;
}

template<typename T> T porters() {
	static const T LN_TWO = boost::math::constants::ln_two<T>(),
		LN_GLAISHER_KINKELIN = log(boost::math::constants::glaisher<T>()),
		LN_PI = log(boost::math::constants::pi<T>()),
		PI_SQR = boost::math::constants::pi_sqr<T>(),
		PORTERS = (6 * LN_TWO * (48 * LN_GLAISHER_KINKELIN
			- LN_TWO - 4 * LN_PI - 2)) / PI_SQR - 0.5;
	return PORTERS;
}

template<typename T> T prince_ruperts_cube() {
	static const T ROOT_TWO = boost::math::constants::root_two<T>(),
		PRINCE_RUPERTS_CUBE = (3 * ROOT_TWO) / 4;
	return PRINCE_RUPERTS_CUBE;
}

template<typename T> T pythagoras() {
	static const T PYTHAGORAS = boost::math::constants::root_two<T>();
	return PYTHAGORAS;
}

template<typename T> T robbins() {
	static const T PI = boost::math::constants::pi<T>(),
		ROOT_TWO = boost::math::constants::root_two<T>(),
		ROOT_THREE = boost::math::constants::root_three<T>(),
		ROBBINS = ((4 + 17 * ROOT_TWO - 6 * ROOT_THREE - 7 * PI) / 105)
			+ (log1p(ROOT_TWO) / 5) + ((2 * log(2 + ROOT_THREE)) / 5);
	return ROBBINS;
}

template<typename T> T sierpinski_k() {
	static const T PI = boost::math::constants::pi<T>(),
		PI_CUBED = boost::math::constants::pi_cubed<T>(),
		EULER = boost::math::constants::euler<T>(),
		SIERPINSKI_K = PI * log((4 * PI_CUBED * exp(2 * EULER))
			/ pow(boost::math::tgamma<T>(0.25), 4));
	return SIERPINSKI_K;
}

template<typename T> T sierpinski_s() {
	static const T PI_CUBED = boost::math::constants::pi_cubed<T>(),
		EULER = boost::math::constants::euler<T>(),
		SIERPINSKI_S = log((4 * PI_CUBED * exp(2 * EULER))
			/ pow(boost::math::tgamma<T>(0.25), 4));
	return SIERPINSKI_S;
}

template<typename T> T silver_ratio() {
	static const T SILVER_RATIO = boost::math::constants::root_two<T>() + 1;
	return SILVER_RATIO;
}

template<typename T> T theodorus() {
	static const T THEODORUS = boost::math::constants::root_three<T>();
	return THEODORUS;
}

template<typename T> T twenty_vertex_entropy() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		TWENTY_VERTEX_ENTROPY = (3 * ROOT_THREE) / 2;
	return TWENTY_VERTEX_ENTROPY;
}

template<typename T> T weierstrass() {
	static const T ROOT_PI = boost::math::constants::root_pi<T>(),
		PI = boost::math::constants::pi<T>(),
		WEIERSTRASS = (exp2(static_cast<T>(1.25)) * ROOT_PI
			* exp(PI / 8)) / pow(boost::math::tgamma<T>(0.25), 2);
	return WEIERSTRASS;
}

template<typename T> T wylers() {
	static const T PI = boost::math::constants::pi<T>(),
		WYLERS = (9 / (8 * pow(PI, 4))) * pow(pow(PI, 5) / 1920, 0.25);
	return WYLERS;
}

template<typename T> T zero() {
	static const T ZERO = 0;
	return ZERO;
}

} // namespace constant

namespace constant {
namespace func {

// use std::uint_fastNN_t if you believe it helps
template<typename T, typename A = unsigned>
T champernowne(const A & b = 10) {
	T c = 0, last;

	for(std::size_t n = 1; n == 1 || c != last; ++n) {
		T sub = 0;

		for(std::size_t k = 1; k <= n; ++k) {
			sub += floor(log(k) / log(b));
		}

		last = c;
		c += n / pow(b, n + sub);
	}

	return c;
}

// throws error if r is negative and even
template<typename T, typename A = unsigned>
T favard(const A & r = 2) {
	static const T PI = boost::math::constants::pi<T>();

	if (r == 0) {
		return 1;
	} else if (r % 2) { // odd (dirichlet lambda)
		return (4 / PI) * ((1 - exp2(-(static_cast<T>(r) + 1)))
			* boost::math::zeta<T>(r + 1));
	} else { // even (dirichlet beta)
		return (-4 / PI) * (pow(-2, static_cast<T>(-2) * (r + 1))
			/ boost::math::tgamma<T>(r + 1))
			* (boost::math::polygamma<T>(r, 0.25)
			- boost::math::polygamma<T>(r, 0.75));
	}
}

} // namespace func
} // namespace constant

#endif // CONSTANTS_HPP_


#include <cmath>
#include <iostream>
#include <vector>
#include <unordered_map>
#include <boost/functional/hash.hpp>

struct Coord {
    int x = 0;
    int y = 0;

    bool operator== (const Coord& o) const {
        return x == o.x && y == o.y;
    }
};

struct pair_hash {
    size_t operator()(const Coord& c) const {
        size_t seed = 0;
        boost::hash_combine(seed, c.x);
        boost::hash_combine(seed, c.y);
        return seed;
    }
};
using Map = std::unordered_map<Coord, uint64_t, pair_hash>;
Map gMap;

void report(const Coord& c) {
    std::cout << "X: " << c.x << ", Y: " << c.y << ", V: " << gMap[c] << "\n";
}

uint64_t sumNeigh(const Coord& c) {
    static Coord null{0, 0};
    if (c == null) {
        return 1;
    }
    return gMap[{c.x, c.y + 1}]
         + gMap[{c.x, c.y - 1}]
         + gMap[{c.x + 1, c.y + 1}]
         + gMap[{c.x + 1, c.y - 1}]
         + gMap[{c.x - 1, c.y + 1}]
         + gMap[{c.x - 1, c.y - 1}]
         + gMap[{c.x - 1, c.y}]
         + gMap[{c.x + 1, c.y}];
}

void Spiral(int X, int Y, int target) {
    int x, y, dx, dy;
    x = y = dx = 0;
    dy         = -1;
    int t      = std::max(X, Y);
    int maxI   = t * t;
    for (int i = 0; i < maxI; i++) {
        if ((-X / 2 <= x) && (x <= X / 2) && (-Y / 2 <= y) && (y <= Y / 2)) {
            Coord c{x, y};
            auto V = gMap[c] = sumNeigh(c);
            if (V > target) {
                report(c);
                return;
            }
        }
        if ((x == y) || ((x < 0) && (x == -y)) || ((x > 0) && (x == 1 - y))) {
            t  = dx;
            dx = -dy;
            dy = t;
        }
        x += dx;
        y += dy;
    }
}

int main() {
    std::vector<int> inputs{325489};
    for (auto n : inputs) {
        std::cout << "input: " << n << "; ";
        Spiral(800, 800, n);
    }
}


#include <igl/doublearea.h>
#include <igl/internal_angles.h>
#include <igl/is_irregular_vertex.h>
#include <igl/readOBJ.h>
#include <igl/PI.h>
#include <Eigen/Core>
#include <iostream>

#include "tutorial_shared_path.h"

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  MatrixXd V;
  MatrixXi F;

  igl::readOBJ(TUTORIAL_SHARED_PATH "/horse_quad.obj",V,F);

  // Count the number of irregular vertices, the border is ignored
  vector<bool> irregular = igl::is_irregular_vertex(V,F);

  int vertex_count = V.rows();
  int irregular_vertex_count =
    std::count(irregular.begin(),irregular.end(),true);
  double irregular_ratio = double(irregular_vertex_count)/vertex_count;

  printf("Irregular vertices: \n%d/%d (%.2f%%)\n",
    irregular_vertex_count,vertex_count, irregular_ratio*100);

  // Compute areas, min, max and standard deviation
  VectorXd area;
  igl::doublearea(V,F,area);
  area = area.array() / 2;

  double area_avg   = area.mean();
  double area_min   = area.minCoeff() / area_avg;
  double area_max   = area.maxCoeff() / area_avg;
  double area_sigma = sqrt(((area.array()-area_avg)/area_avg).square().mean());

  printf("Areas (Min/Max)/Avg_Area Sigma: \n%.2f/%.2f (%.2f)\n",
    area_min,area_max,area_sigma);

  // Compute per face angles, min, max and standard deviation
  MatrixXd angles;
  igl::internal_angles(V,F,angles);
  angles = 360.0 * (angles/(2*igl::PI)); // Convert to degrees

  double angle_avg   = angles.mean();
  double angle_min   = angles.minCoeff();
  double angle_max   = angles.maxCoeff();
  double angle_sigma = sqrt( (angles.array()-angle_avg).square().mean() );

  printf("Angles in degrees (Min/Max) Sigma: \n%.2f/%.2f (%.2f)\n",
    angle_min,angle_max,angle_sigma);

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	assert(int(size(v)) == m * n);
	Vector w(m * n);

	for (int i= 0; i < m; i++)
	    for (int j= 0; j < n; j++) {
		int k= i * n + j; // offset
		w[k]= 4 * v[k];
		if (i > 0) w[k]-= v[k-n];   // upper neighbor
		if (i < m-1) w[k]-= v[k+n]; // lower neighbor
		if (j > 0) w[k]-= v[k-1];   // left neighbor
		if (j < n-1) w[k]-= v[k+1]; // right neighbor
	    }
	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

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

    mtl::compressed2D<double> A0;
    laplacian_setup(A0, 4, 5);
    cout << "A0 is\n" << A0 << endl;

    mtl::dense_vector<double> v(20);
    iota(v);
    cout << "v is " << v << endl;

    mtl::dense_vector<double> w1(A0 * v);
    cout << "A0 * v is " << w1 << endl;

    poisson2D_dirichlet A(4, 5);
    mtl::dense_vector<double> w2(A * v);
    cout << "A * v is " << w2 << endl;

    w1-= w2;
    if (one_norm(w1) > 0.001) throw "Wrong result";

    return 0;
}


#include <armadillo>

#include <mona/utility.hpp>
#include <mona/line.hpp>
#include <mona/targets/window.hpp>
#include <mona/axes.hpp>
#include <mona/colors.hpp>

auto fit(const arma::fvec& x, const arma::fvec& y) -> arma::fvec
{
    auto xx = x - arma::mean(x);
    auto yy = y - arma::mean(y);
    float b = arma::sum(xx % yy) / arma::sum(xx % xx);
    float a = arma::mean(y) - b * arma::mean(x);
    return a + b * x;
}

auto random(arma::fvec x, float mean, float std)
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> dis(mean, std);

    for(auto& e: x)
    {
        e += dis(gen);
    }

    return x;
}

int main()
{
    arma::fvec x = mona::linspace(-4, 4, 15);
    arma::fvec y = random(x, 0, 0.9);

    auto target = mona::targets::window();
    auto cam = mona::camera();
    auto axes = mona::axes();

    auto dots = mona::dots(x, y, mona::colors::tomato);
    auto line = mona::line(x, fit(x, y), mona::colors::tomato);

    float delta = 0;
    while (target.active())
    {
        axes.submit(dots);
        axes.submit(line);

        cam = target.control_camera(cam);
        target.submit(axes);
        target.draw();
    }
}


//////////////////////////////////
//
// Compile: g++ -std=c++14 -O3 ex5.cxx -o ex5 -lboost_timer
//
//////////////////////////////////

#include <iostream>		// std::cout
#include <fstream>		// std::ifsteam
#include <utility>		// std::pair
#include <vector>		// std::vector
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/timer/timer.hpp>

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

  if (argc != 2)
  {
    std::cerr << "No file!!!" << std::endl;
    return -1;
  }

  std::ifstream file(argv[1]); // std::ifstream::in ??
  std::string str;

  boost::timer::cpu_timer timer;

  /* start get number of edges */
  getline(file, str);

  using namespace boost::spirit;
  using qi::int_;
  using qi::double_;
  using qi::parse;

  int n;

  auto it = str.begin();
  bool r = parse(it, str.end(),
		 int_[([&n](int i){n = i;})] >> int_);
  /* end get number of edges */

  /* start get list of edges and weights */
  typedef std::pair<int, int> Edge;
  std::vector<Edge> Edges; // vector to store std::pair of edges.
  std::vector<int> Weights; // vector to weights as integers.

  while (getline(file,str)){ // get graph line-by-line.
    int Vert1;
    int Vert2;
    double Weight; // are all weights integer-valued ??

    auto it = str.begin(); // initialize iterator for qi::parse.

    bool r = parse(it, str.end(), // parse graph.
		 int_[([&Vert1](int i){Vert1 = i;})] >> qi::space >> int_[([&Vert2](int i){Vert2 = i;})] >> qi::space >> double_[([&Weight](double i){Weight = i;})]);

    Edge edge = std::make_pair(Vert1, Vert2); // make edge-pair out of vertices.
    Edges.push_back(edge);
    Weights.push_back(Weight);
  }
  /* end get list of edges and weights */

  typedef boost::property<boost::edge_weight_t, double> EdgeWeightProperty; // initialize type to store weights on edges.
  // adjacency_list<out-edges, vertex_set, directedness, vertex properties, edge properties>
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProperty> Graph; // create graph.

  Graph g(Edges.begin(),Edges.end(), Weights.begin(), n); // populate graph.

  // here, the graph should be built... Find shortest path.
  typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  vertex_descriptor source = boost::vertex(1, g); // define source vertex as vertex with index == 1.
  std::vector<vertex_descriptor> parents(boost::num_vertices(g)); // initialize vectors for predecessor and distances.
  std::vector<int> distances(boost::num_vertices(g));

  // find shortest distances.
  boost::dijkstra_shortest_paths(g, source, boost::predecessor_map(&parents[0]).distance_map(&distances[0]));

  /* start find longest-shortest path */
  int maxDistance = 0;
  int maxVertex = 0;

  // create iterator over vertices.
  // vertexPair.first is the iterated element, and .second is the end-index of all vertices.
  typedef boost::graph_traits <Graph>::vertex_iterator vertex_iter;
  std::pair<vertex_iter, vertex_iter> vertexPair;

  for (vertexPair = boost::vertices(g); vertexPair.first != vertexPair.second; ++vertexPair.first) // vertexPair = boost::vertices loops over all vertices in g.
  {
    // replace maxDistance if a greater distance is found, and maxDistance must be less than "infinity" (of 32-bit signed integer).
    if ((distances[*vertexPair.first] > maxDistance) && (distances[*vertexPair.first] < 2147483647)){
      maxDistance = distances[*vertexPair.first];
      maxVertex = *vertexPair.first;
    }
    // if distance == maxDistance, check if vertex index is smaller.
    if ((distances[*vertexPair.first] == maxDistance) && (*vertexPair.first < maxVertex)){
      maxVertex = *vertexPair.first;
    }
  }

  boost::timer::cpu_times times = timer.elapsed();

  // output vertex and distance of the longest-shortest path.
  std::cout << "RESULT VERTEX " << maxVertex << std::endl;
  std::cout << "RESULT DIST " << maxDistance <<  std::endl;
  /* end find longest-shortest path */

  // print CPU- and Wall-Time.
  // boost::timer::cpu_times returns tuple of wall, system and user times in nanoseconds.
  std::cout << std::endl;
  std::cout << "WALL-CLOCK " << times.wall / 1e9 << "s" << std::endl;
  std::cout << "USER TIME " << times.user / 1e9 << "s" << std::endl;

  file.close();
  return 0;
}


/**
 * \file SinusGeneratorFilter.cpp
 */

#include "SinusGeneratorFilter.h"

#include <cmath>

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

namespace ATK
{
  template<typename DataType_>
  SinusGeneratorFilter<DataType_>::SinusGeneratorFilter()
  :Parent(0, 2), volume(1), offset(0), frequency(0), frequ_cos(1), frequ_sin(0), cos(1), sin(0)
  {
  }

  template<typename DataType_>
  SinusGeneratorFilter<DataType_>::~SinusGeneratorFilter()
  {
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_frequency(DataType_ frequency)
  {
    this->frequency = frequency;
    this->frequ_cos = std::cos(2 * boost::math::constants::pi<DataType_>() * frequency / output_sampling_rate);
    this->frequ_sin = std::sin(2 * boost::math::constants::pi<DataType_>() * frequency / output_sampling_rate);
    Parent::setup();
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return frequency;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::full_setup()
  {
    Parent::full_setup();
    cos = 1;
    sin = 0;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::process_impl(int64_t size) const
  {
    DataType* ATK_RESTRICT output_cos = outputs[0];
    DataType* ATK_RESTRICT output_sin = outputs[1];

    for(int64_t i = 0; i < size; ++i)
    {
      auto new_cos = cos * frequ_cos - sin * frequ_sin;
      auto new_sin = cos * frequ_sin + sin * frequ_cos;
      auto norm = (new_cos * new_cos + new_sin * new_sin);

      cos = new_cos / norm;
      sin = new_sin / norm;

      *(output_cos++) = volume * cos + offset;
      *(output_sin++) = volume * sin + offset;
    }
  }

  template class SinusGeneratorFilter<float>;
  template class SinusGeneratorFilter<double>;
}


//
// Copyright (c) 2019 INRIA
//

#include <pinocchio/math/rotation.hpp>
#include <pinocchio/math/sincos.hpp>
#include <Eigen/Geometry>

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

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_toRotationMatrix)
{
  using namespace pinocchio;
  const int max_tests = 1e5;
  for(int k = 0; k < max_tests; ++k)
  {
    const double theta = std::rand();
    double cos_value, sin_value;

    pinocchio::SINCOS(theta,&sin_value,&cos_value);
    Eigen::Vector3d axis(Eigen::Vector3d::Random().normalized());

    Eigen::Matrix3d rot; toRotationMatrix(axis,cos_value,sin_value,rot);
    Eigen::Matrix3d rot_ref = Eigen::AngleAxisd(theta,axis).toRotationMatrix();

    BOOST_CHECK(rot.isApprox(rot_ref));
  }
}

BOOST_AUTO_TEST_SUITE_END()


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

// Set the parameters |i|, |j|, and |k| to be the indices of the grid cell
// containing the point |p| for a grid with lower corner |lc| and grid cell
// width (spacing) |dx|.
inline void floor(const Eigen::Vector3d& p, const Eigen::Vector3d& lc,
                  double dx, int& i, int& j, int& k) {
  // Compute |p|'s location relative to |lc|.
  // Dividing by |dx| yields a 3D vector indicating the number of grid
  // cells (including fractions of grid cells, as the vector elements are
  // floating-point values) away from |lc| that |p| is located.
  Eigen::Vector3d p_lc_over_dx = (p - lc) / dx;

  // Set the indices to the floor of |p_lc_over_dx|'s elements.
  i = static_cast<int>(p_lc_over_dx[0]);
  j = static_cast<int>(p_lc_over_dx[1]);
  k = static_cast<int>(p_lc_over_dx[2]);
}

int main(int argc, char** argv) {
  // Make a grid with spacing of 2 with lower corner (1, 2, 3).
  double dx = 2.0;
  Eigen::Vector3d lc(1, 2, 3);

  // (i, j, k) should be (floor((6-1)/2), floor((4-2)/2), floor((3-3)/2)),
  // which is (floor(2.5), floor(1), floor(0)) = (2, 1, 0).
  Eigen::Vector3d p1(6, 4, 3);
  int i, j, k;
  floor(p1, lc, dx, i, j, k);
  std::cout << "i = " << i << ", j = " << j << ", k = " << k << std::endl;

  // (i, j, k) should be (3, 8, 9).
  Eigen::Vector3d p2(7, 18, 22);
  floor(p2, lc, dx, i, j, k);
  std::cout << "i = " << i << ", j = " << j << ", k = " << k << std::endl;

  // (i, j, k) should be (-1, 0, 0)--wait, negative indices aren't
  // allowed, right?!
  Eigen::Vector3d p3(-1, 2, 3);
  floor(p3, lc, dx, i, j, k);
  std::cout << "i = " << i << ", j = " << j << ", k = " << k << std::endl;

  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/convex_hull_2.h>
#include <CGAL/ch_jarvis.h>
#include <CGAL/ch_eddy.h>
#include <CGAL/Timer.h>

#include <CGAL/point_generators_2.h>
#include <CGAL/algorithm.h>
#include <boost/math/special_functions/next.hpp>

#include <iostream>
#include <vector>

#define bench(METHOD,CONTAINER) \
{\
  std::size_t previous=0;\
  unsigned run=0;\
  CGAL::Timer time;\
  do{\
    result.clear();\
    time.start();\
    METHOD( CONTAINER.begin(), CONTAINER.end(), std::back_inserter(result) );\
    time.stop();\
    if( previous!=0 && previous!=result.size()) std::cerr << "error got different result" << std::endl;\
    previous=result.size();\
  }while(++run<repeat+1);\
  std::cout << result.size() << " points on the convex hull using "<< #METHOD << "; Done in "<< time.time() << "s\n";\
}

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef std::vector<Point_2> Points;
typedef CGAL::Creator_uniform_2<double,Point_2>  Creator;

int main(int argc, char** argv)
{
  unsigned nbpts=100000;
  unsigned repeat=0;
  unsigned seed=0;

  if (argc>1)  nbpts=atoi(argv[1]);

  if (argc>2)  repeat=atoi(argv[2]);
  if (argc>3)  seed=atoi(argv[3]);

  Points points, result;

  CGAL::Random r(seed);
  CGAL::Random_points_in_disc_2<Point_2,Creator> g( 150.0,r);
  CGAL::cpp11::copy_n( g, nbpts, std::back_inserter(points));

  //the following code is for testing when there is only two extreme points, affine hull is 2D
  /*
  CGAL::Bbox_2 bbox=points.begin()->bbox();
  for (Points::iterator it=points.begin();it!=points.end();++it)
    bbox=bbox+it->bbox();

  points.push_back( Point_2(bbox.xmin()-1,bbox.ymin()-1) );
  points.push_back( Point_2(bbox.xmax()+1,bbox.ymax()+1) );
  */

  //the following code is for testing when there is only three extreme points
  /*
  CGAL::Bbox_2 bbox=points.begin()->bbox();
  for (Points::iterator it=points.begin();it!=points.end();++it)
    bbox=bbox+it->bbox();

  points.push_back( Point_2(bbox.xmin()-1,bbox.ymin()-1) );
  points.push_back( Point_2(bbox.xmax()+1,bbox.ymax()+1) );
  points.push_back( Point_2(bbox.xmax(),bbox.ymax()+2) );
  */

  //the following code is for testing when there is only two extreme points, affine hull is 1D
  /*
  points.clear();
  for (unsigned i=0;i<nbpts;++i)
    points.push_back(Point_2(i,i));
  */

  std::cout << "seed is " << seed << "; using " << nbpts << " pts; on " << repeat+1 <<  " run(s).\n";

  std::cout << "Using vector" << std::endl;
  bench(CGAL::convex_hull_2,points)
  //bench(CGAL::ch_akl_toussaint,points)
  //bench(CGAL::ch_bykat,points)
  //bench(CGAL::ch_eddy,points)
  //bench(CGAL::ch_graham_andrew,points)
  //bench(CGAL::ch_jarvis,points)

  {
    std::list<Point_2> pt_list;
    std::copy(points.begin(),points.end(),std::back_inserter(pt_list));
    std::cout << "Using list" << std::endl;
    bench(CGAL::convex_hull_2,pt_list)
  }
}


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>
#include <vector>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Construct_theta_graph_2.h>
#include <CGAL/property_map.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

// select the kernel type
typedef CGAL::Exact_predicates_inexact_constructions_kernel   Kernel;
typedef Kernel::Point_2                   Point_2;
typedef Kernel::Direction_2               Direction_2;

/* define the struct for edge property */
struct Edge_property {
  /* record the Euclidean length of the edge */
  double euclidean_length;
};

// define the Graph (e.g., to be undirected,
// and to use Edge_property as the edge property, etc.)
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::undirectedS,
                              Point_2,
                              Edge_property>  Graph;

int main(int argc, char ** argv)
{
  const std::string fname = argc!=3 ? "data/n20.cin" : argv[2];
  unsigned int k = argc!=3 ? 6 : atoi(argv[1]);

  if (argc != 3) {
    std::cout << "Usage: " << argv[0] << " <no. of cones> <input filename>" << std::endl;
    std::cout << "Using default values " << k << " " << fname << "\n";
  }

  if (k<2) {
    std::cout << "The number of cones should be larger than 1!" << std::endl;
    return 1;
  }
  // open the file containing the vertex list
  std::ifstream inf(fname);
  if (!inf) {
    std::cout << "Cannot open file " << argv[1] << "!" << std::endl;
    return 1;
  }

  // iterators for reading the vertex list file
  std::istream_iterator< Point_2 > input_begin( inf );
  std::istream_iterator< Point_2 > input_end;

  // initialize the functor
  // If the initial direction is omitted, the x-axis will be used
  CGAL::Construct_theta_graph_2<Kernel, Graph> theta(k);
  // create an adjacency_list object
  Graph g;
  // construct the theta graph on the vertex list
  theta(input_begin, input_end, g);

  // select a source vertex for dijkstra's algorithm
  boost::graph_traits<Graph>::vertex_descriptor v0;
  v0 = vertex(0, g);
  std::cout << "The source vertex is: " << g[v0] << std::endl;

  std::cout << "The index of source vertex is: " << v0 << std::endl;

  // calculating edge length in Euclidean distance and store them in the edge property
  boost::graph_traits<Graph>::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
    boost::graph_traits<Graph>::edge_descriptor e = *ei;
    boost::graph_traits<Graph>::vertex_descriptor  u = source(e, g);
    boost::graph_traits<Graph>::vertex_descriptor  v = target(e, g);
    const Point_2& pu = g[u];
    const Point_2& pv = g[v];

    double dist = CGAL::sqrt( CGAL::to_double(CGAL::squared_distance(pu,pv)) );
    g[e].euclidean_length = dist;
    std::cout << "Edge (" << g[u] << ", " << g[v] << "): " << dist << std::endl;
  }

  // calculating the distances from v0 to other vertices
  boost::graph_traits<Graph>::vertices_size_type n = num_vertices(g);
  // vector for storing the results
  std::vector<double> distances(n);
  // Calling the Dijkstra's algorithm implementation from boost.
  boost::dijkstra_shortest_paths(g,
                                 v0,
                                 boost::weight_map(get(&Edge_property::euclidean_length, g)).
                                 distance_map(CGAL::make_property_map(distances)) );

  std::cout << "distances are:" << std::endl;
  for (unsigned int i=0; i < n; ++i) {
    std::cout << "distances[" << i << "] = " << distances[i] << ", (x,y)=" << g[vertex(i, g)];
    std::cout << " at Vertex " << i << std::endl;
  }

  return 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_POW_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POW_HPP_INCLUDED

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

 /*!

    @ingroup group-exponential
    Function object implementing pow capabilities

    Computes \f$x^y\f$

    @par Semantic:

    For every parameters of floating types respectively T, U:

    @code
    T r = pow(x,y);
    @endcode

    is similar to:

    @code
    T r = exp(y*log(x));
    @endcode

    The pow function is conformant to std:pow considering the limits behaviours
    defined by the standard:

         - pow(+0, y), where y is a negative odd integer, returns +Inf
         - pow(-0, y), where y is a negative odd integer, returns -Inf
         - pow(+/-0, y), where y is negative, finite, and is an even integer or a non-integer, returns +Inf
         - pow(+/-0, -Inf) returns +inf
         - pow(+0, y), where y is a positive odd integer, returns +0
         - pow(-0, y), where y is a positive odd integer, returns -0
         - pow(+/-0, y), where y is positive non-integer or a positive even integer, returns +0
         - pow(-1, +/-Inf) returns 1
         - pow(+1, y) returns 1 for any y, even when y is Nan
         - pow(x, +/-0) returns 1 for any x, even when x is Nan
         - pow(x, y) returns Nan if x is finite and negative and y is finite and non-integer.
         - pow(x, -Inf) returns +Inf for any |x|<1
         - pow(x, -Inf) returns +0 for any |x|>1
         - pow(x, +Inf) returns +0 for any |x|<1
         - pow(x, +Inf) returns +Inf for any |x|>1
         - pow(-Inf, y) returns -0 if y is a negative odd integer
         - pow(-Inf, y) returns +0 if y is a negative non-integer or even integer
         - pow(-Inf, y) returns -Inf if y is a positive odd integer
         - pow(-Inf, y) returns +Inf if y is a positive non-integer or even integer
         - pow(+Inf, y) returns +0 for any negative y
         - pow(+Inf, y) returns +Inf for any positive y

      But return a value of the same type as the first parameter, which is necessary for good SIMD behaviour.

      @par Decorators

        -std_ decorator provides access to std:pow

        -fast_ decorator almost nearly uses the naive formula and so doesnot really care for limits and leads to lower

  **/
  Value pow(Value const & v0, Value const& y);
} }
#endif

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

#endif


// IIR_Butterworth.cpp : This file contains the 'main' function. Program execution begins and ends there.
//Calculate the coefficients of the butterworth filter. It follows the steps as described in Matlab.
//The same symbols as in Matlab are used to facilitate the writing of the code
#define _CRTDBG_MAP_ALLOC
#include <stdio.h>
#include <stdlib.h>
#include <crtdbg.h>
#include <iostream>
#include <math.h>
#include <string.h>
#include <vector>
#include <complex.h>
#include "IIR_Butterworth.h"

#define ARMA_DONT_USE_CXX11
#include <armadillo>

#ifdef _DEBUG
#define DBG_NEW new ( _NORMAL_BLOCK , __FILE__ , __LINE__ )
// Replace _NORMAL_BLOCK with _CLIENT_BLOCK if you want the
// allocations to be of _CLIENT_BLOCK type
#else
#define DBG_NEW new
#endif

#define PI 3.141592653589793

int main()
{

    double f1 = 100;  //High Pass
    double f2 = 300; //Low Pass
    double sf = 2048;    //Sampling frequency
    int order_filt = 2; //Order
    double Nyquist_F = sf / 2;

    std::vector<std::vector<double> > coeff_final(2);

    int type_filt = 0;
    IIR_B_F::IIR_Butterworth ir_b;

    bool check_stability_flag;

    //_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
    //_CrtSetBreakAlloc(154);
    //_CrtSetBreakAlloc(155);
    //_CrtSetBreakAlloc(156);

    int coeff_numb = 0;
    switch (type_filt)
    {
    case 0:

        coeff_numb = 2*(size_t)order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();

        coeff_final = ir_b.lp2bp(f1 / Nyquist_F, f2 / Nyquist_F, order_filt);

       check_stability_flag =  ir_b.check_stability_iir(coeff_final);

       if (check_stability_flag)
       {

           std::cout << "The filter is stable" << std::endl;

       }

       else
       {

           std::cout << "The filter is unstable" << std::endl;

       }

        for (int kk = 0; kk < 2; kk++)
        {
            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {
                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;

        }

        break;

    case 1:

        coeff_numb = 2 * order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();
        coeff_final = ir_b.lp2bs(f1 / Nyquist_F, f2 / Nyquist_F, order_filt);

        check_stability_flag = ir_b.check_stability_iir(coeff_final);

        if (check_stability_flag)
        {

            std::cout << "The filter is stable" << std::endl;

        }

        else
        {

            std::cout << "The filter is unstable" << std::endl;

        }

        for (int kk = 0; kk < 2; kk++)
        {

            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {

                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;

        }

        break;

    case 2:

        coeff_numb = order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();
        coeff_final = ir_b.lp2hp(f1 / Nyquist_F, order_filt);

        check_stability_flag = ir_b.check_stability_iir(coeff_final);

        if (check_stability_flag)
        {

            std::cout << "The filter is stable" << std::endl;

        }

        else
        {

            std::cout << "The filter is unstable" << std::endl;

        }

        for (int kk = 0; kk < 2; kk++)
        {

            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {

                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;
        }

        break;

    case 3:
        coeff_numb = order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();
        coeff_final = ir_b.lp2lp(f2 / Nyquist_F, order_filt);

        check_stability_flag = ir_b.check_stability_iir(coeff_final);

        if (check_stability_flag)
        {

            std::cout << "The filter is stable" << std::endl;

        }

        else
        {

            std::cout << "The filter is unstable" << std::endl;

        }

        for (int kk = 0; kk < 2; kk++)
        {

            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {

                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;

        }

        break;

    }

     //Create a complex sine wave
    std::vector<double> test_sin(sf,0.0);
    double sf_1 = 20;
    double sf_2 = 60;
    for (double kk = 0; kk < sf; kk++)
    {

            test_sin[kk] = sin(2*PI*kk*sf_1/sf) + sin(2*PI*kk*sf_2/sf);

    }

    std::vector<double> filt_sign = ir_b.Filter_Data(coeff_final,test_sin);

}


// Copyright (c) 2013, Manuel Blum
// All rights reserved.

// Define this symbol to enable runtime tests for allocations
//#define EIGEN_RUNTIME_NO_MALLOC

#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <cstdio>
#include <string>
#include <ctime>
#include "nn.h"

using namespace std;

inline void swap(int &val)
{
	val = (val << 24) | ((val << 8) & 0x00ff0000) | ((val >> 8) & 0x0000ff00) | (val >> 24);
}

matrix_t read_mnist_images(std::string filename)
{

	matrix_t X;
	std::ifstream fs(filename.c_str(), std::ios::binary);
	if (fs)
	{
		int magic_number, num_images, num_rows, num_columns;
		fs.read((char *)&magic_number, sizeof(magic_number));
		fs.read((char *)&num_images, sizeof(num_images));
		fs.read((char *)&num_rows, sizeof(num_rows));
		fs.read((char *)&num_columns, sizeof(num_columns));
		if (magic_number != 2051)
		{
			swap(magic_number);
			swap(num_images);
			swap(num_rows);
			swap(num_columns);
		}

		X = matrix_t::Zero(num_images, num_rows * num_columns);

		for (size_t i = 0; i < num_images; ++i)
		{
			for (size_t j = 0; j < num_rows * num_columns; ++j)
			{
				unsigned char temp = 0;
				fs.read((char *)&temp, sizeof(temp));
				X(i, j) = (double)temp;
			}
		}
		fs.close();
	}
	else
	{
		std::cout << "error reading file: " << filename << std::endl;
		exit(1);
	}
	return X;
}

matrix_t read_mnist_labels(std::string filename)
{
	matrix_t Y;
	std::ifstream fs(filename.c_str(), std::ios::binary);
	if (fs)
	{
		int magic_number, num_images, num_rows, num_columns;
		fs.read((char *)&magic_number, sizeof(magic_number));
		fs.read((char *)&num_images, sizeof(num_images));
		if (magic_number != 2049)
		{
			swap(magic_number);
			swap(num_images);
		}

		Y = matrix_t::Zero(num_images, 10);

		for (size_t i = 0; i < num_images; ++i)
		{
			unsigned char temp = 0;
			fs.read((char *)&temp, sizeof(temp));
			Y(i, (int)temp) = 1.0;
		}
		fs.close();
	}
	else
	{
		std::cout << "error reading file: " << filename << std::endl;
		exit(1);
	}
	return Y;
}

double measure_accuracy(matrix_t Y, matrix_t Y_test)
{
	int correct = 0;
	for (int i = 0; i < Y.rows(); i++)
	{
		int j = 0;
		for (; j < Y.cols(); j++)
		{
			int out = Y_test(i, j) > 0.7 ? 1 : 0;
			//printf("%3i   %4.4f", (int)Y(i, j), Y_test(i, j));

			if (out != Y(i, j))
				break;
		}
		//cout << endl;
		if (j == Y.cols())
		{
			correct++;
		}
		else
		{
			cout << Y_test.row(i) << endl;
		}
	}
	//cout << "Correct answer " << correct << " among " << Y.rows() << " test" << endl;
	double accuracy = (double)correct / (double)Y.rows() * 100;
	return accuracy;
	//cout << "Accuracy: " <<  accuracy << "%" << endl;
}

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

	if (argc != 2)
	{
		std::cout << "please provide path to mnist data ..." << std::endl;
		std::cout << "you can download the dataset at http://yann.lecun.com/exdb/mnist/" << std::endl;
		std::cout << std::endl
				  << "usage: " << argv[0] << " path_to_data" << std::endl
				  << std::endl;
		return 1;
	}

	std::string path = argv[1];

	std::cout << "reading data" << std::endl;

	matrix_t X_train = read_mnist_images(path + "/train-images.idx3-ubyte");
	matrix_t Y_train = read_mnist_labels(path + "/train-labels.idx1-ubyte");
	matrix_t X_test = read_mnist_images(path + "/t10k-images.idx3-ubyte");
	matrix_t Y_test = read_mnist_labels(path + "/t10k-labels.idx1-ubyte");
	// number of optimization steps
	int max_steps = 5;
	// regularization parameter
	double lambda = 0.0;

	// specify network topology
	Eigen::VectorXi topo(3);
	topo << X_train.cols(), 300, Y_test.cols();
	std::cout << "topology: " << topo.transpose() << std::endl;

	// initialize a neural network with given topology
	std::cout << "initializing network" << std::endl;
	NeuralNet nn(topo);

	std::cout << "scaling the data" << std::endl;
	nn.autoscale(X_train, Y_train);

	//std::cout << Y_train << std::endl;
	// train the network
	std::cout << "starting training" << std::endl;
	std::cout << "iter        error" << std::endl;
	double err;
	clock_t begin = clock();
	for (int i = 0; i < max_steps; ++i)
	{
		err = nn.loss(X_train, Y_train, lambda);
		nn.rprop();
		printf("%4i   %10.7f\n", i, err);
	}
	clock_t end = clock();
	double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	cout << "Training time: " << elapsed_secs << endl;

	// test accuracy
	nn.forward_pass(X_test);
	matrix_t prediction = nn.get_activation();
	int correct = 0;
	int k;
	for (size_t i = 0; i < Y_test.rows(); ++i)
	{
		prediction.row(i).maxCoeff(&k);
		correct += Y_test(i, k);
	}

	std::cout << "test accuracy: " << correct * 1.0 / Y_test.rows()*100 << "% " << std::endl;
	//cout << prediction << endl;
	// cout << "*****************Printing the wrong output***************" << endl;
	// double accuracy = measure_accuracy(Y_test, prediction);
	// std::cout << "Accuracy: " << accuracy << std::endl;
	nn.write("mnist.nn");

	return 0;
}


#include <cmath>
#include <memory>
#include <iostream>
#include <random>

#include <Eigen/Core>

#include "RandomGenerator.h"
#include "mcmc/MCMC.h"

int main()
{
  // function handle, can be C++11 lambda, regular function pointer, or std::function
  auto logdens = [](const Eigen::VectorXd& x) {
    return -(x(0) * x(0)) - (x(1) * x(1));
  };

  auto rng = std::make_shared<RandomGenerator>();

  auto mcmc = std::make_shared<MCMC>(logdens, 2, rng);

  mcmc->Run(10000);

  auto chain = mcmc->GetChain();

  std::cout << "Mean: " << chain.colwise().mean() << std::endl;

  std::cout << "Autocorrelation: " << mcmc->GetAutocorrelation(10).transpose() << std::endl;

  return 0;
}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/rotation_ex.cpp
 *  @author Jeferson Lima
 *  @brief  Rotation Matrix Example
 *  @date   Mar 7, 2022
 **/

#include <iostream>
#include <Eigen/Core>

void rot2dVec(const Eigen::Matrix<float, 4, 1>& v,
              Eigen::Matrix<float, 4, 1>& r,
	      float theta)
{

  Eigen::Matrix<float, 4, 4> rotation;
  rotation << std::cos(theta), -std::sin(theta), 0.0, 0.0,
              std::sin(theta), std::cos(theta),  0.0, 0.0,
	      0.0,             0.0,              1.0, 0.0,
	      0.0,             0.0,              0.0, 1.0;

  std::cout << "v values: \n" << v << std::endl;
  std::cout << "rotation values: \n" << rotation << std::endl;

  r = rotation * v;

}

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

  Eigen::Matrix<float, 4, 1> v;
  Eigen::Matrix<float, 4, 1> r;
  float theta;

  // init matrix
  v << 0.5, 0.5, 0.0, 1.0;
  theta = 0.785398; // 45 degree -> radian

  rot2dVec(v, r, theta);

  std::cout << "r values:\n" << r << std::endl;

  return 0;
}


#ifndef TRIUMF_NMR_UTILITIES_HPP
#define TRIUMF_NMR_UTILITIES_HPP

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

#include <triumf/constants/codata_2018.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// nuclear magnetic resonance
namespace nmr {

// miscellaneous helper functions
namespace utilities {

// calculate the gyromagnetic ratio (MHz / T) of an NMR probe nucleus from its
// magnetic dipole moment (nm) and spin quantum number
template <typename T>
constexpr T calculate_gamma(T magnetic_dipole_moment, T spin_quantum_number) {
  return 1e6 * boost::math::constants::two_pi<T>() *
         triumf::constants::codata_2018::nuclear_magneton_in_MHz_T<T>::value() *
         magnetic_dipole_moment / spin_quantum_number;
}

} // namespace utilities

} // namespace nmr

} // namespace triumf

#endif // TRIUMF_NMR_UTILITIES_HPP


#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <vector>
#include <cmath>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
using namespace Eigen;

string TrajectoryFile = "../compare.txt";
void ICP(vector<Vector3d> pts_est, vector<Vector3d> pts_gt, Matrix3d &R, Vector3d &t);
void DrawTrajectory(vector<Sophus::SE3d> poses1, vector<Sophus::SE3d> poses2, string winName="Trajectory Viewer");

int main(){
    ifstream fin(TrajectoryFile);
    vector<Vector3d> pts_est,pts_gt;
    vector<Sophus::SE3d> poses_gt, poses_est;
    while(!fin.eof()){
        double time, tx,ty,tz,qx,qy,qz,qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Quaterniond q(qw,qx,qy,qz);
        Vector3d t_est(tx,ty,tz);
        Sophus::SE3d SE3_est(q,t_est);
        pts_est.push_back(t_est);
        poses_est.push_back(SE3_est);

        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Quaterniond q2(qw,qx,qy,qz);
        Vector3d t_gt(tx,ty,tz);
        Sophus::SE3d SE3_gt(q2,t_gt);
        poses_gt.push_back(SE3_gt);
        pts_gt.push_back(t_gt);
    }
    Matrix3d R;
    Vector3d t;
    ICP(pts_est, pts_gt, R, t);
    Sophus::SE3d SE3_E2G(R, t);

    int N = poses_gt.size();
    vector<Sophus::SE3d> poses_after(N);
    for(int i=0;i<N;i++){
        poses_after[i] = SE3_E2G*poses_est[i];
    }
    DrawTrajectory(poses_gt, poses_est, "before");
    DrawTrajectory(poses_gt, poses_after, "after");

    double error=0;
    for (int i=0;i<N;i++){
        Sophus::SE3d SE3_differ = poses_gt[i].inverse()*poses_after[i];
        typedef Eigen::Matrix<double, 6, 1> Vector6d;
        Vector6d se3_differ = SE3_differ.log();
        error += se3_differ.dot(se3_differ);
    }
    double RMSE = sqrt(error/N);
    cout<<"RMSE: "<<RMSE<<endl;

    return 0;
}

void ICP(vector<Vector3d> pts_est, vector<Vector3d> pts_gt, Matrix3d &R, Vector3d &t){
    Vector3d pe(0,0,0), pg(0,0,0);
    int N = pts_est.size();
    for(int i=0;i<N;i++){
        pe += pts_est[i];
        pg += pts_gt[i];
    }
    pe /= N;
    pg /= N;
    vector<Vector3d> q_est(N), q_gt(N);
    for (int i=0;i<N;i++){
        q_est[i] = pts_est[i]-pe;
        q_gt[i] = pts_gt[i]-pg;
    }
    Matrix3d W = Matrix3d::Zero();
    for (int i=0;i<N;++i){
        W += q_gt[i]*q_est[i].transpose();
    }
    cout<<"W="<<W<<endl;
    JacobiSVD<Matrix3d> svd(W, ComputeFullU|ComputeFullV);
    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;
    Matrix3d R_ = U*(V.transpose());
    if(R_.determinant()<0){
        R_ = -R_;
    }

    Vector3d t_ = pg-R_*pe;
    R = R_;
    t = t_;
}

void DrawTrajectory(vector<Sophus::SE3d> poses1, vector<Sophus::SE3d> poses2, string winName) {

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind(winName, 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

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

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

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses1.size() - 1; i++) {
            glColor3f(1, 0.0f, 0);
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        for (size_t i = 0; i < poses2.size() - 1; i++) {
            glColor3f(0, 0.0f, 1);
            glBegin(GL_LINES);
            auto p1 = poses2[i], p2 = poses2[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/math/basic/mathematicalConstants.h"

namespace tudat
{
namespace unit_tests
{

// Define Boost test suite.
BOOST_AUTO_TEST_SUITE( test_mathematical_constants )

//! Check correct pi using PI = circumference / diameter:
BOOST_AUTO_TEST_CASE( test_PI )
{
    const double radius = 5.0;

    // Circumference circle with radius 5, 32 digits precision, see
    // http://www.wolframalpha.com/input/?i=Circumference+of+a+circle+with+radius+5
    // http://www.wolframalpha.com/input/?i=N[10*PI,66]
    double circumference = 31.4159265358979323846264338327950288419716939937510582097494459230;
    BOOST_CHECK_CLOSE(  mathematical_constants::PI,
                        circumference / ( 2.0 * radius ) ,
                        std::numeric_limits< double >::epsilon( ) );
}

//! Check correct E using E Wolfram alpha as reference
BOOST_AUTO_TEST_CASE( test_E )
{
    // Numerical value from:
    // http://www.wolframalpha.com/input/?i=e+72+digits
    BOOST_CHECK_CLOSE( mathematical_constants::E,
        2.71828182845904523536028747135266249775724709369995957496696762772407663,
        std::numeric_limits< double >::epsilon( ) );
}

//! Check correct GOLDEN_RATIO using GOLDEN_RATIO Wolfram alpha as reference
BOOST_AUTO_TEST_CASE( test_GOLDEN_RATIO )
{
    // Numerical value from:
    // http://www.wolframalpha.com/input/?i=golden+ratio+72+digits
    BOOST_CHECK_CLOSE(  mathematical_constants::GOLDEN_RATIO,
        1.618033988749894848204586834365638117720309179805762862135448622705260463,
        std::numeric_limits< double >::epsilon( ) );
}

//! Check correct NAN using boost Floating Point Classification (fpclassify)
BOOST_AUTO_TEST_CASE( test_NAN )
{
    // Numerical value from:
    // http://www.wolframalpha.com/input/?i=golden+ratio+72+digits
    BOOST_CHECK( boost::math::isnan( TUDAT_NAN ) );
}

BOOST_AUTO_TEST_CASE( test_TemplatedValues )
{
    double one = 1.0;
    long double longOne = 1.00000000000000000000L;

    BOOST_CHECK_CLOSE(  mathematical_constants::getFloatingInteger< double >( 1 ),
                        one, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE(  mathematical_constants::getFloatingInteger< long double >( 1 ),
                        longOne, std::numeric_limits< long  double >::epsilon( ) );

    BOOST_CHECK_SMALL(  mathematical_constants::getFloatingInteger< double >( 0 ),
                        std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_SMALL(  mathematical_constants::getFloatingInteger< long double >( 0 ),
                        std::numeric_limits< long  double >::epsilon( ) );

    double two = 2.0;
    long double longTwo = 2.00000000000000000000L;

    BOOST_CHECK_CLOSE(  mathematical_constants::getFloatingInteger< double >( 2 ),
                        two, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE(  mathematical_constants::getFloatingInteger< long double >( 2 ),
                        longTwo, std::numeric_limits< long  double >::epsilon( ) );

    BOOST_CHECK_CLOSE(  mathematical_constants::getFloatingFraction< double >( 1, 2 ),
                        mathematical_constants::getFloatingInteger< double >( 1 ) /
                        mathematical_constants::getFloatingInteger< double >( 2 ),
                        std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE(  mathematical_constants::getFloatingFraction< long double >( 1, 2 ),
                        mathematical_constants::getFloatingInteger< long double >( 1 ) /
                        mathematical_constants::getFloatingInteger< long double >( 2 )
                        , std::numeric_limits< long double >::epsilon( ) );

    BOOST_CHECK_CLOSE(  mathematical_constants::getPi< double >( ),
                        mathematical_constants::PI, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE(  mathematical_constants::getPi< long double >( ),
                        mathematical_constants::LONG_PI, std::numeric_limits< long  double >::epsilon( ) );

}

// Close Boost test suite.
BOOST_AUTO_TEST_SUITE_END( ) // End test_mathematical_constants

} // namespace unit_tests
} // namespace tudat


#include <iostream>
#include <array>
#include <cmath>
#include <tuple>
#include <cassert>
#include <NTL/ZZ.h>
#include <NTL/vector.h>
#include <random>
#include <chrono>

using namespace std;
using namespace NTL;

//ex. compile:
//g++ -o <exe> remainder_alg_first_pass.cpp -std=c++11 -march=native -O2 -lntl -lgmp -lgmpxx -pthread

// to compile and run on linux use
// g++ -o <executable file name> remainder_alg_first_pass.cpp
// ./<executable file name>

//note that everywhere it says 'int' we will eventually want to use GMP or whatnot
//ints are too small...

//translates indices in trees to indices in arrays
//if you want left child of (i,j), do (i+1, 2*j)
//right child is (i+1, 2*j+1), and parent is (i-1, j//2)
int flatten_index(int i, int j)
{
	assert (j <= pow(2,i) - 1);
	return pow(2,i) +j-1;
}

//gives tree indices from array index
tuple<int, int> lift_index(int k)
{
	int i = log2(k+1);
	int j = k + 1 - pow(2,i);

	return make_tuple(i,j);
}

void print_tree(Vec<ZZ> & X)
{
	cout << endl;
	for (int i = 0; i < X.length(); i++)
	{
		cout << X[i] << " , ";
	}
	cout << endl;
}

ZZ mtree_val(int i, int j, const Vec<ZZ> &m)
{
	int N = m.length();
	int depth = log2(N);
	assert ((i <= depth) and (j < pow(2, i)));

	if (i == depth)
	{
		return m[j];
	}

	else
	{
		return mtree_val(i+1, 2*j, m)*mtree_val(i+1, 2*j+1, m);
	}
}

ZZ Atree_val(int i, int j, const Vec<ZZ> &A)
{
	int N = A.length();
	int depth = log2(N);
	assert ((i <= depth) and (j < pow(2,i)));

	if ((i == depth) or (j == 0))
	{
		return A[j];
	}

	else
	{
		return Atree_val(i+1, 2*j-1, A)*Atree_val(i+1, 2*j, A);
	}
}

//k will be approx log log N (it's the space-time tradeoff)
//when k = 0 we have normal remainder tree
Vec<ZZ> RemTree(const Vec<ZZ> &A, const Vec<ZZ> &m, const ZZ &V = ZZ(1))
{
	int N = A.length();
	assert (N == m.length());
	int depth = log2(N);

	//assume the input is of size a power of 2
	Vec<ZZ> mtree;
	mtree.SetLength(2*N-1);

	//initialize leaves
	for (int j = 0; j < pow(2, depth); j++)
	{
		int leaf = flatten_index(depth,j);
		mtree[leaf] = m[j];
	}

	//build up the product tree recursively
	for (int i = depth-1; i >= 0; i--)
	{
		for (int j = 0; j < pow(2,i); j++)
		{
			int parent = flatten_index(i,j);
			int left = 2*parent + 1;
			int right = left + 1;

			//padding is not necessary if on this step we catch IndexError with 1
			mtree[parent] = mtree[left]*mtree[right];
		}
	}

	Vec<ZZ> Atree;
	Atree.SetLength(2*N-1);

	//initialize leaves
	for (int j = 0; j < pow(2, depth); j++)
	{
		int leaf = flatten_index(depth, j);
		Atree[leaf] = A[j];
	}

	//build up the 'product' tree recursively
	for (int i = depth-1; i >= 0; i--)
	{
		int leftmost = flatten_index(i,0);
		int leftmost_child = 2*leftmost + 1;
		Atree[leftmost] = A[leftmost_child];

		for (int j = 1; j < pow(2, i); j++)
		{
			int parent = flatten_index(i,j);
			int superleft = 2*parent;
			int left = superleft + 1;

			Atree[parent] = Atree[superleft]*Atree[left];
		}
	}

	Vec<ZZ> remtree;
	remtree.SetLength(2*N-1);

	remtree[0] = (V*A[0])%mtree[0];

	for (int i = 0; i < depth; i++)
	{
		//notice it goes every two!
		for (int j = 0; j < pow(2, i); j++)
		{
			int parent = flatten_index(i,j);
			int left = 2*parent + 1;
			int right = left + 1;

			remtree[left] = remtree[parent]%mtree[left];
			remtree[right] = (remtree[parent]*Atree[right])%mtree[right];
		}
	}

	Vec<ZZ> remainders;
	remainders.SetLength(N);

	int leftmost_leaf = flatten_index(depth, 0);
	for (int j = 0; j < N; j++)
	{
		remainders[j] = remtree[leftmost_leaf+j];
	}
	return remainders;
}

//searches up to N for Wilson primes
Vec<ZZ> WilsonRemainders(long N, long interval)
{
	Vec<ZZ> A;
	Vec<ZZ> m;

	A.SetLength(interval);
	m.SetLength(interval);

	Vec<ZZ> remainders;
	remainders.SetLength(N);

	ZZ V = ZZ(1);

	for (int b = 0; b*interval < N; b++)
	{
		for (int i = 0; i < interval; i++)
		{
			ZZ val = ZZ(b*interval+(i+1));

			A[i] = val-1;
			if (val == 1)
			{
				A[i] += 1;
			}

			if (ProbPrime(val))
			{
				m[i] = val*val;
			}

			else
			{
				m[i] = 1;
			}
		}

		Vec<ZZ> interval_remainders = RemTree(A, m, V);
		for (int i = 0; i < interval; i++)
		{
			remainders[b*interval+i] = interval_remainders[i];
			V *= A[i];
		}
	}

	return remainders;
}

int main()
{
	long N = pow(2,1);

	Vec<ZZ> A;
	Vec<ZZ> m;

	A.SetLength(N);
	m.SetLength(N);

	random_device rd;
	mt19937 mt(rd());
	uniform_int_distribution<int> dist(1, N);

	for(int i = 0; i < N; i++){
		int bitsize = log2(i+1)+2;

		A[i] = dist(mt);
		m[i] = dist(mt);

	}

	//Vec<ZZ> ztree = zero_vector(2*N);

	//print_tree(A);
	//print_tree(m);

	auto start = chrono::high_resolution_clock::now();
	Vec<ZZ> remainders = RemTree(A,m);
	Vec<ZZ> WREM = WilsonRemainders(pow(2,18), pow(2,18));
	auto finish = chrono::high_resolution_clock::now();

	chrono::duration<double> runtime = finish-start;

	//print_tree(WREM);
	cout << remainders[0] << endl;
	cout << WREM[562] << " , " << 563*563 << endl;
	cout << runtime.count() << endl;

}


// Compression methods for vectors
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <vector_compression/vector_compression.h>

#include <Eigen/Core>

#include "contrib/angles.h"

#include <iostream>

#define DEBUG 0

namespace vector_compression
{

// The "hemi-oct" compression scheme is taken from
//  Cigolle, Zina H., Sam Donow, and Daniel Evangelakos.
//  "A survey of efficient representations for independent unit vectors."
//  Journal of Computer Graphics Techniques Vol 3.2 (2014).

// Assume normalized input on +Z hemisphere.
// Output is [-1, 1]
static Eigen::Vector2f float32x3_to_hemioct(const Eigen::Vector3f& v)
{
	// Project the hemisphere onto the hemi-octahedron,
	// and then project into the xy plane
	Eigen::Vector2f p = v.head<2>() * (1.0f / (fabsf(v.x()) + fabsf(v.y()) + v.z()));

	// Rotate and scale the center diamond to the unit square
	return Eigen::Vector2f(p.x() + p.y(), p.x() - p.y());
}

static Eigen::Vector3f hemioct_to_float32x3(const Eigen::Vector2f& e)
{
	// Rotate and scale the unit square back to the center diamond
	Eigen::Vector2f temp = 0.5f * Eigen::Vector2f(e.x() + e.y(), e.x() - e.y());
	Eigen::Vector3f v;
	v << temp, 1.0 - fabsf(temp.x()) - fabsf(temp.y());

	return v.normalized();
}

int32_t float_to_snormb(float v, int b)
{
	return std::round(std::min(1.0f, std::max(-1.0f, v)) * ((1 << (b-1)) - 1));
}

float snormb_to_float(int32_t v, int b)
{
	// Sign extend
	if(v & (1 << (b-1)))
		v |= 0xFFFFFFFF & ~((1 << b) - 1);

	return std::min(1.0f, std::max(-1.0f, ((float)v) / ((1 << (b-1)) - 1)));
}

void encodeQuaternion(const Eigen::Quaternionf& quat, uint8_t* dest)
{
	// Recover rotation angle & axis
	float angle = 2.0f * acosf(quat.w());
	angle = angles::normalize_angle(angle);

	float norm = sqrtf(1.0f - quat.w()*quat.w());

	Eigen::Vector3f axis;

	if(norm != 0)
	{
		axis << quat.x() / norm, quat.y() / norm, quat.z() / norm;
	}
	else
		axis << 0.0f, 0.0f, 0.0f;

	// Map to the upper hemisphere
	if(axis.z() < 0)
	{
		axis *= -1.0f;
		angle = -angle;
	}

	Eigen::Vector2f hemi = float32x3_to_hemioct(axis);

	int32_t hemi_a = float_to_snormb(hemi.x(), 12);
	int32_t hemi_b = float_to_snormb(hemi.y(), 12);

	int32_t angle_sn = float_to_snormb(angle / M_PI, 16);

#if DEBUG
	printf("encode: %f, %f, %f, angle_sn: %u (0x%X)\n", hemi.x(), hemi.y(), angle, angle_sn, angle_sn);
	printf("angle scale: %d\n", ((1 << (16-1)) - 1));
#endif

	dest[0] = hemi_a;
	dest[1] = ((hemi_a >> 8) & 0xF) | ((hemi_b & 0xF) << 4);
	dest[2] = hemi_b >> 4;
	dest[3] = angle_sn;
	dest[4] = angle_sn >> 8;
}

Eigen::Quaternionf decodeQuaternion(const uint8_t* src)
{
	int32_t hemi_a = src[0] | (((uint32_t)src[1] & 0xF) << 8);
	int32_t hemi_b = (src[1] >> 4) | (((uint32_t)src[2]) << 4);
	int32_t angle_sn = src[3] | (((uint32_t)src[4]) << 8);

	Eigen::Vector2f hemi(
		snormb_to_float(hemi_a, 12), snormb_to_float(hemi_b, 12)
	);

	Eigen::Vector3f axis = hemioct_to_float32x3(hemi);

	float angle = snormb_to_float(angle_sn, 16) * M_PI;

#if DEBUG
	printf("decode: %f, %f, %f, angle_sn: %d\n", hemi.x(), hemi.y(), angle, angle_sn);
#endif

	return Eigen::Quaternionf(Eigen::AngleAxisf(angle, axis));
}

Eigen::Vector3f latticeToVector(const Eigen::Vector3i& lattice, float r)
{
	return r * Eigen::Vector3f(
		lattice.y() + lattice.z(),
		lattice.x() + lattice.z(),
		lattice.x() + lattice.y()
	);
}

Eigen::Vector3f alignedLatticeToVector(const Eigen::Vector3i& lattice, float r)
{
	Eigen::Vector3f m = lattice.cast<float>();

	Eigen::Vector3f x = m + m.y() * Eigen::Vector3f::UnitY() + ((lattice.x() + lattice.z()) % 2) * Eigen::Vector3f::UnitY();

	x *= r;

	return x;
}

static inline int sgn(float x)
{
	if(x >= 0)
		return 1;
	else
		return -1;
}

static inline float roundFloat(float x)
{
	return std::round(x);
}

Eigen::Vector3i vectorToAlignedLattice(const Eigen::Vector3f& p, float r)
{
	Eigen::Vector3f m = p / r;

	Eigen::Vector3f cartesian_lattice = m.unaryExpr(std::ptr_fun(roundFloat));
	Eigen::Vector3i lattice_int = cartesian_lattice.cast<int>();

	if(lattice_int.sum() % 2 != 0)
	{
		Eigen::Vector3f local = m - cartesian_lattice;
		int c;
		local.array().abs().maxCoeff(&c);

		lattice_int[c] += sgn(local[c]);
	}

	lattice_int.y() = (lattice_int.y() - ((lattice_int.x() + lattice_int.z()) % 2)) / 2;

	return lattice_int;
}

Eigen::Vector3i vectorToLattice(const Eigen::Vector3f& p, float r)
{
	Eigen::Vector3f normalized = p / r;

	Eigen::Vector3f cartesian_lattice = normalized.unaryExpr(std::ptr_fun(roundFloat));
	Eigen::Vector3i lattice_int = cartesian_lattice.cast<int>();

	if(lattice_int.sum() % 2 != 0)
	{
		Eigen::Vector3f local = normalized - cartesian_lattice;
		int c;
		local.array().abs().maxCoeff(&c);

		lattice_int[c] += sgn(local[c]);
	}

	Eigen::Matrix3i inv;
	inv <<
		-1, 1, 1,
		1, -1, 1,
		1, 1, -1
	;

	return (inv * lattice_int) / 2;
}

void encode3DVector6(const Eigen::Vector3f& vec, float extent, uint8_t* dest)
{
	// We are using 16 bit resolution per lattice index.
	float lattice_r = extent / (2.0f * ((1 << 15)-1));

	Eigen::Vector3i lattice = vectorToLattice(vec, lattice_r);

	for(int i = 0; i < 3; ++i)
	{
		lattice[i] = std::min(32767, std::max(-32768, lattice[i]));
		dest[2*i] = lattice[i];
		dest[2*i+1] = lattice[i] >> 8;
	}
}

Eigen::Vector3f decode3DVector6(float extent, const uint8_t* src)
{
	// We are using 16 bit resolution per lattice index.
	float lattice_r = extent / (2.0f * ((1 << 15)-1));

	Eigen::Vector3i lattice;
	for(int i = 0; i < 3; ++i)
	{
		int16_t idx = src[2*i] | (((uint16_t)src[2*i+1]) << 8);
		lattice[i] = idx;
	}

	return latticeToVector(lattice, lattice_r);
}

VectorCompression::VectorCompression(unsigned int bits)
 : m_bits(bits)
{
	setExtent(Eigen::Vector3f(1.0, 1.0, 1.0));
}

void VectorCompression::setExtent(const Eigen::Vector3f& extent)
{
	m_extent = extent;

	m_r = powf(sqrtf(2.0) * extent.x() * extent.y() * extent.z() / (1LL << m_bits), 1.0f / 3.0f) * sqrtf(2.0);

	m_latticeExtent.x() = std::floor(extent.x() / m_r);
	m_latticeExtent.y() = std::floor(extent.y() / m_r / 2.0f);
	m_latticeExtent.z() = std::floor(extent.z() / m_r);

	uint64_t high = 8 * m_latticeExtent.x() * m_latticeExtent.y() * m_latticeExtent.z();
	if(high > (1ULL << m_bits))
	{
		std::cerr << "lattice overflow: " << high << " > " << (1LL << 24) << "\n";
		std::cerr << "calculated radius: " << m_r;
		throw std::runtime_error("lattice overflow");
	}
}

uint64_t VectorCompression::compress(const Eigen::Vector3f& vec) const
{
	Eigen::Vector3i lattice = vectorToAlignedLattice(vec, m_r);

	// Shift to [0, 2*m_latticeExtent]
	lattice += m_latticeExtent;

	// Constrain
	lattice.x() = std::max(0, std::min(2*m_latticeExtent.x(), lattice.x()));
	lattice.y() = std::max(0, std::min(2*m_latticeExtent.y(), lattice.y()));
	lattice.z() = std::max(0, std::min(2*m_latticeExtent.z(), lattice.z()));

	uint64_t index = lattice.x() + lattice.y() * 2 * m_latticeExtent.x() + lattice.z() * 4 * m_latticeExtent.x() * m_latticeExtent.y();

	// Sanity check
	if(index >= (1ULL << m_bits))
	{
		std::cerr << "lattice overflow while compressing " << vec.transpose() << "\n";
		std::cerr << "radius: " << m_r << "\n";
		std::cerr << "lattice extents: " << m_latticeExtent.transpose() << "\n";
		std::cerr << "lattice coords: " << lattice.transpose() << "\n";
		std::cerr << "=> index: " << index << "\n";
		index = (1 << m_bits) - 1;
	}

	return index;
}

Eigen::Vector3f VectorCompression::uncompress(uint64_t compressed) const
{
	Eigen::Vector3i lattice;

	lattice.z() = compressed / (4 * m_latticeExtent.x() * m_latticeExtent.y());

	uint64_t remainder = compressed % (4 * m_latticeExtent.x() * m_latticeExtent.y());

	lattice.y() = remainder / (2*m_latticeExtent.x());
	lattice.x() = remainder % (2*m_latticeExtent.x());

	// Shift to [-extent, extent]
	lattice -= m_latticeExtent;

	return alignedLatticeToVector(lattice, m_r);
}

float VectorCompression::guaranteedPrecision() const
{
	return sqrt(2.0f) * m_r;
}

}


#include <medusa/Medusa_fwd.hpp>
#include <Eigen/SparseCore>
#include <Eigen/IterativeLinearSolvers>

/// Basic medusa example, we are solving 2D Poisson's equation on unit square
/// with Dirichlet boundary conditions.
/// http://e6.ijs.si/medusa/wiki/index.php/Poisson%27s_equation

using namespace mm;  // NOLINT

int main() {
    // Create the domain and discretize it
    BoxShape <Vec2d> box(0.0, 1.0);
    double dx = 0.01;
    DomainDiscretization <Vec2d> domain = box.discretizeWithStep(dx);

    // Find support for the nodes
    int N = domain.size();
    domain.findSupport(FindClosest(9));  // the support for each node is the closest 9 nodes

    // Construct the approximation engine, in this case a weighted least squares using monomials as
    // basis functions, no weight, and scale to farthest
    int m = 2;  // basis order
    WLS <Monomials<Vec2d>, NoWeight<Vec2d>,
    ScaleToFarthest> wls(Monomials<Vec2d>::tensorBasis(m));  // tensor basis of monomials
    // up to order 2,
    // {1,x,x2,y,yx,yx2,y2,y2x,y2x2}

    // compute the shapes (we only need the Laplacian) using our WLS
    auto storage = domain.computeShapes<sh::lap>(wls);

    Eigen::SparseMatrix<double, Eigen::RowMajor> M(N, N);
    Eigen::VectorXd rhs(N); rhs.setZero();
    M.reserve(storage.supportSizes());

    // construct implicit operators over our storage
    auto op = storage.implicitOperators(M, rhs);
    auto op3 = storage.implicitVectorOperators(M, rhs);

    ExplicitVectorOperators<decltype(storage)> op2(storage);

    for (int i : domain.interior()) {
        double x = domain.pos(i, 0);
        double y = domain.pos(i, 1);
        // set the case for nodes in the domain

        // op.lap(i) = -2 * PI * PI * std::sin(PI * x) * std::sin(PI * y);
        op2.div(op2.grad(i, {-1, -1, -1}),{-1, -1, -1}) = -2 * PI * PI * std::sin(PI * x) * std::sin(PI * y);
        // op.div(epsilon(i) * op.grad(i)) = epsilon(i) -3 * PI * PI * std::sin(PI * x) * std::sin(PI * y);
    }
    for (int i : domain.boundary()) {
        // enforce the boundary conditions
        op.value(i) = 0.0;
    }

    Eigen::BiCGSTAB<decltype(M), Eigen::IncompleteLUT<double>> solver;
    solver.compute(M);
    ScalarFieldd u = solver.solve(rhs);

    // Write the solution into file
    std::ofstream out_file("poisson_dirichlet_2D_data.m");
    out_file << "positions = " << domain.positions() << ";" << std::endl;
    out_file << "solution = " << u << ";" << std::endl;
    out_file.close();
    return 0;
}