#include <Eigen/StdVector>
#include <unsupported/Eigen/BVH>
#include <iostream>

using namespace Eigen;
typedef AlignedBox<double, 2> Box2d;

Box2d ei_bounding_box(const Vector2d &v) { return Box2d(v, v); } //compute the bounding box of a single point

struct PointPointMinimizer //how to compute squared distances between points and rectangles
{
  PointPointMinimizer() : calls(0) {}
  typedef double Scalar;

  double minimumOnVolumeVolume(const Box2d &r1, const Box2d &r2) { ++calls; return r1.squaredExteriorDistance(r2); }
  double minimumOnVolumeObject(const Box2d &r, const Vector2d &v) { ++calls; return r.squaredExteriorDistance(v); }
  double minimumOnObjectVolume(const Vector2d &v, const Box2d &r) { ++calls; return r.squaredExteriorDistance(v); }
  double minimumOnObjectObject(const Vector2d &v1, const Vector2d &v2) { ++calls; return (v1 - v2).squaredNorm(); }

  int calls;
};

int main()
{
  typedef std::vector<Vector2d, aligned_allocator<Vector2d> > StdVectorOfVector2d;
  StdVectorOfVector2d redPoints, bluePoints;
  for(int i = 0; i < 100; ++i) { //initialize random set of red points and blue points
    redPoints.push_back(Vector2d::Random());
    bluePoints.push_back(Vector2d::Random());
  }

  PointPointMinimizer minimizer;
  double minDistSq = std::numeric_limits<double>::max();

  //brute force to find closest red-blue pair
  for(int i = 0; i < (int)redPoints.size(); ++i)
    for(int j = 0; j < (int)bluePoints.size(); ++j)
      minDistSq = std::min(minDistSq, minimizer.minimumOnObjectObject(redPoints[i], bluePoints[j]));
  std::cout << "Brute force distance = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;

  //using BVH to find closest red-blue pair
  minimizer.calls = 0;
  KdBVH<double, 2, Vector2d> redTree(redPoints.begin(), redPoints.end()), blueTree(bluePoints.begin(), bluePoints.end()); //construct the trees
  minDistSq = BVMinimize(redTree, blueTree, minimizer); //actual BVH minimization call
  std::cout << "BVH distance         = " << sqrt(minDistSq) << ", calls = " << minimizer.calls << std::endl;

  return 0;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <iostream>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>

int
main()
{
  using namespace boost;
  // ID numbers for the routers (vertices).
  enum
  { A, B, C, D, E, F, G, H, n_vertices };
  const int n_edges = 11;
  typedef std::pair < int, int >Edge;

  // The list of connections between routers stored in an array.
  Edge edges[] = {
  Edge(A, B), Edge(A, C),
        Edge(B, D), Edge(B, E), Edge(C, E), Edge(C, F), Edge(D, H),
        Edge(D, E), Edge(E, H), Edge(F, G), Edge(G, H)
  };

  // Specify the graph type and declare a graph object
  typedef edge_list < Edge*, Edge, std::ptrdiff_t, std::random_access_iterator_tag> Graph;
  Graph g(edges, edges + n_edges);

  // The transmission delay values for each edge.
  float delay[] =
    {5.0, 1.0, 1.3, 3.0, 10.0, 2.0, 6.3, 0.4, 1.3, 1.2, 0.5};

  // Declare some storage for some "external" vertex properties.
  char name[] = "ABCDEFGH";
  int parent[n_vertices];
  for (int i = 0; i < n_vertices; ++i)
    parent[i] = i;
  float distance[n_vertices];
  std::fill(distance, distance + n_vertices, std::numeric_limits < float >::max());
  // Specify A as the source vertex
  distance[A] = 0;

  bool r = bellman_ford_shortest_paths(g, int (n_vertices),
                                       weight_map(make_iterator_property_map
                                                  (&delay[0],
                                                   get(edge_index, g),
                                                   delay[0])).
                                       distance_map(&distance[0]).
                                       predecessor_map(&parent[0]));

  if (r)
    for (int i = 0; i < n_vertices; ++i)
      std::cout << name[i] << ": " << distance[i]
        << " " << name[parent[i]] << std::endl;
  else
    std::cout << "negative cycle" << std::endl;

  return EXIT_SUCCESS;
}


#ifndef MLA_SOLVERS_CONJUGATE_GRADIENT_HPP
#define MLA_SOLVERS_CONJUGATE_GRADIENT_HPP

#include <string>

#include <boost/lexical_cast.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>

#include <mla/operations/level1/dot.h++>
#include <mla/operations/level1/axpy.h++>
#include <mla/operations/level1/scale.h++>
#include <mla/operations/level2/gemv.h++>

#include <mla/solvers/SolverReturnCodes.h++>

#include <mla/output.h++>

namespace mla
{

/**
 * Conjugate gradient method algorithm
 *
 *@param A	Matrix
 *@param x	unknown vector
 *@param b	vector
 *@param delta	error tolerance
 *@param max_iterations	maximum number of iterations allowed
 *
 *@return
 */
template<typename Scalar, template<typename> class MatrixStoragePolicy, template<typename> class VectorStoragePolicyX, template<typename> class VectorStoragePolicyB >
ReturnCode
cg(MatrixStoragePolicy<Scalar> &A, VectorStoragePolicyX<Scalar> &x, VectorStoragePolicyB<Scalar> &b, const double delta, int max_iterations)
{
	if( !A.isSquare() )
	{
		throw LAException("cg: A must be a square matrix");
	}

	if(A.columns() != b.size())
	{
		throw LAException("cg: A.columns() != b.size()");
	}

	x.resize( b.size() );

	VectorStoragePolicyB<Scalar> r, p(A.columns());
	VectorStoragePolicyB<Scalar> Ap(A.columns());
	Scalar dotrr, dotrrnew, alpha;

	//r = b - A*x;
	r = b;
	mla::gemv( (Scalar)(-1.0f), A, x, (Scalar)(1.0f), r );
	p = r;

	dotrr = dot(r,r);

	for (int iter = 0; iter < max_iterations; iter++)
	{
		//Ap = A*p;
		mla::gemv( (Scalar)1.0f, A, p, (Scalar)1.0f, Ap);

		alpha = dotrr/dot(p,Ap);
		//x = x + alpha*p;
		mla::axpy(alpha, p, x);

		// r_{k+1] = r_{k} - a*A*p
		mla::axpy( (Scalar)-1.0f*alpha, Ap, r);

		dotrrnew = dot(r,r);

		if(dotrrnew < delta)
		{
			return OK;
		}

		//p = r + (dotrrnew/dotrr)*p;
		mla::scale( dotrrnew/dotrr, p);
		mla::axpy( (Scalar)1.0f, r, p);

		dotrr = dotrrnew;
	}

	return ERR_EXCESSIVE_ITERATIONS;
}

}

#endif


#include <memory>
#include <algorithm>
#include <vector>
#include "lib.h"
#include "expression.h"
#include "phase_expression.h"
#include "phase_solver.h"
#include <thread>
#include <iterator>
#include "exprtk.hpp"

#include <boost/numeric/odeint.hpp>
#include <cmath>

using Variables = exprtk::symbol_table<double>;
using Formula = exprtk::expression<double>;
using Parser = exprtk::parser<double>;

using state_type = std::vector<double>;

class CauchyProblem {

public:
    CauchyProblem(double init_x, double init_y):
        _init_x(init_x), _init_y(init_y) { };

    void operator()(const state_type & x, state_type & dxdt, double t) {
        dxdt[0] = 2 * std::exp(pow(t, 2)) - x[0] / t;
    };

private:
    double _init_x;
    double _init_y;
};

double ode(double x, double t) {
    return 2 * std::exp(pow(t, 2)) - x / t;;
}

double runge_kutta_step(double x, double t, double step) {
    auto k1 = ode(x, t);
    auto k2 = ode(x + step / 2 * k1, t + step / 2);
    auto k3 = ode(x + step / 2 * k2, t + step / 2);
    auto k4 = ode(x + step * k3, t + step);
    return x + step / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
}

auto expression = std::make_shared<PhaseExpression>(
        1,
        "2*exp(t^2)-x/t",
        std::make_pair(0.0, 0.0));

double runge_kutta_step_expr(double x, double t, double step) {

    auto k1 = expression->evaluate(x, t);
    auto k2 = expression->evaluate(x + step / 2 * k1, t + step / 2);
    auto k3 = expression->evaluate(x + step / 2 * k2, t + step / 2);
    auto k4 = expression->evaluate(x + step * k3, t + step);
    return x + step / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
}

void cauchy_problem(const state_type & x, state_type & dxdt, double t) {
    dxdt[0] = 2 * std::exp(pow(t, 2)) - x[0] / t;
};

int main (int argc, char * argv[])
{
    const double t = 1;
    const double m_x = std::exp(1);
    const double step = 0.0000001;

    // auto system = CauchyProblem(1, std::exp(1));
    // auto stepper = boost::numeric::odeint::runge_kutta4<state_type>();

    // state_type x { std::exp(1) };
    //     // state_type x { 27.2991 };

    // boost::numeric::odeint::integrate_const(stepper, system, x, 1.0, 2.0, step);

    // std::cout<< "The solution at 2.0 is " << x[x.size() - 1] << std::endl;

    // auto expression = std::make_shared<PhaseExpression>(
    //     1,
    //     "2*exp(t^2)-x/t",
    //     std::make_pair(m_x, t));
    // auto solver = PhaseSolver(expression, std::make_pair(m_x, t));
    // auto result = solver.solve(2.0, step);
    // std::cout<< "The solution with CustomSolver at 2.0 is " << result << std::endl;

    auto correctColution = std::exp(pow(2, 2)) / 2;

    auto cur_x = m_x;
    for (double cur_t = t; cur_t <= 2; cur_t += step) {
        cur_x = runge_kutta_step(cur_x, cur_t, step);
    }

    std::cout << "Manual Runge-Kutta at point 2 is " << cur_x << std::endl;
    std::cout << "The right solution is " << correctColution << std::endl;

    return 0;
}


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <fstream>
#include <iostream>
#include <Eigen/Eigenvalues>
#include <rl/math/Matrix.h>
#include <rl/math/Vector.h>

int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlPcaDemo FILE" << std::endl;
		return EXIT_FAILURE;
	}

	std::ifstream data;
	data.open(argv[1]);

	std::size_t rows;
	data >> rows;
	std::size_t cols;
	data >> cols;

	rl::math::Matrix src(rows, cols);

	for (std::size_t i = 0; i < rows; ++i)
	{
		for (std::size_t j = 0; j < cols; ++j)
		{
			data >> src(i, j);
		}
	}

	data.close();

	std::cout << "src = " << std::endl << src << std::endl;

	rl::math::Vector mean(cols);
	mean.setZero();

	for (std::size_t i = 0; i < rows; ++i)
	{
		mean += src.row(i);
	}

	mean /= static_cast<rl::math::Real>(rows);

	std::cout << "mean = " << mean.transpose() << std::endl;

	rl::math::Matrix covariance(cols, cols);
	covariance.setZero();

	for (std::size_t i = 0; i < rows; ++i)
	{
		rl::math::Vector delta = src.row(i).transpose() - mean;
		covariance += delta * delta.transpose();
	}

	covariance /= static_cast<rl::math::Real>(rows) - 1;

	std::cout << "covariance = " << std::endl << covariance << std::endl;

	Eigen::EigenSolver<rl::math::Matrix> eigen(covariance);

	std::cout << "eigenvectors = " << std::endl << eigen.eigenvectors() << std::endl;
	std::cout << "eigenvalues = " << std::endl << eigen.eigenvalues() << std::endl;

	return EXIT_SUCCESS;
}


// SPDX-License-Identifier: MIT
// Copyright (c) 2019-2021 Thomas Vanderbruggen <th.vanderbruggen@gmail.com>

#ifndef SCICPP_CORE_STATS
#define SCICPP_CORE_STATS

#include "scicpp/core/functional.hpp"
#include "scicpp/core/numeric.hpp"
#include "scicpp/core/units/quantity.hpp"

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <limits>
#include <numeric>
#include <tuple>

namespace scicpp::stats {

namespace detail {

template <class Array>
auto quiet_nan() {
    return std::numeric_limits<typename Array::value_type>::quiet_NaN();
}

} // namespace detail

//---------------------------------------------------------------------------------
// amax
//---------------------------------------------------------------------------------

template <class Array>
constexpr auto amax(const Array &f) {
    if (f.empty()) {
        return detail::quiet_nan<Array>();
    }

    return *std::max_element(f.cbegin(), f.cend());
}

//---------------------------------------------------------------------------------
// amin
//---------------------------------------------------------------------------------

template <class Array>
constexpr auto amin(const Array &f) {
    if (f.empty()) {
        return detail::quiet_nan<Array>();
    }

    return *std::min_element(f.cbegin(), f.cend());
}

//---------------------------------------------------------------------------------
// ptp
//---------------------------------------------------------------------------------

template <class Array>
constexpr auto ptp(const Array &f) {
    if (f.empty()) {
        return detail::quiet_nan<Array>();
    }

    const auto [it_min, it_max] = std::minmax_element(f.cbegin(), f.cend());
    return *it_max - *it_min;
}

//---------------------------------------------------------------------------------
// average
//---------------------------------------------------------------------------------

template <class Array1, class Array2>
constexpr auto average(const Array1 &f, const Array2 &weights) {
    if (f.empty() || (f.size() != weights.size())) {
        return detail::quiet_nan<Array1>();
    }

    return inner(f, weights) / sum(weights);
}

//---------------------------------------------------------------------------------
// median
//---------------------------------------------------------------------------------

namespace detail {

// https://stackoverflow.com/questions/1719070/what-is-the-right-approach-when-using-stl-container-for-median-calculation
template <class InputIt>
auto median_inplace(InputIt first, InputIt last) {
    using T = typename std::iterator_traits<InputIt>::value_type;
    using raw_t = units::representation_t<T>;
    const auto size = std::distance(first, last);

    if (size == 0) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    const signed_size_t half = size / 2;
    const auto target = first + half;
    std::nth_element(first, target, last);

    if (size % 2 != 0) { // vector size is odd
        return *target;
    } else {
        const auto max_it = std::max_element(first, first + half);
        return (*max_it + *target) / raw_t{2}; // cf. std::midpoint (C++20)
    }
}

} // namespace detail

template <class InputIt, class Predicate>
auto median(InputIt first, InputIt last, Predicate p) {
    auto v = filter(std::vector(first, last), p);
    return detail::median_inplace(v.begin(), v.end());
}

template <class Array, class Predicate>
auto median(const Array &f, Predicate filter) {
    return median(f.cbegin(), f.cend(), filter);
}

template <class Array>
auto median(Array &&f) {
    if constexpr (std::is_lvalue_reference_v<Array>) {
        auto tmp = f;
        return detail::median_inplace(tmp.begin(), tmp.end());
    } else {
        return detail::median_inplace(f.begin(), f.end());
    }
}

template <class Array>
auto nanmedian(const Array &f) {
    return median(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// mean
//---------------------------------------------------------------------------------

template <class InputIt, class Predicate>
constexpr auto mean(InputIt first, InputIt last, Predicate filter) {
    using T = typename std::iterator_traits<InputIt>::value_type;

    if (std::distance(first, last) == 0) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    const auto [res, cnt] = sum(first, last, filter);
    return res / units::representation_t<T>(cnt);
}

template <class Array, class Predicate>
constexpr auto mean(const Array &f, Predicate filter) {
    return mean(f.cbegin(), f.cend(), filter);
}

template <class Array>
constexpr auto mean(const Array &f) {
    return mean(f, filters::all);
}

template <class Array>
auto nanmean(const Array &f) {
    return mean(f, filters::not_nan);
}

template <class Array, typename T = typename Array::value_type>
constexpr auto tmean(const Array &f,
                     const std::array<T, 2> &limits,
                     const std::array<bool, 2> &inclusive = {true, true}) {
    return mean(f, filters::Trim<T>(limits, inclusive));
}

//---------------------------------------------------------------------------------
// gmean
//---------------------------------------------------------------------------------

template <class Array>
auto gmean(Array &&f) {
    using T = typename std::decay_t<Array>::value_type;

    if (f.empty()) {
        return std::numeric_limits<T>::quiet_NaN();
    }

    if constexpr (units::is_quantity_v<T>) {
        using namespace operators;
        return T(std::exp(mean(log(std::forward<Array>(f) / T(1)))));
    } else {
        return std::exp(mean(log(std::forward<Array>(f))));
    }
}

template <class Array, class Predicate>
auto gmean(Array &&f, Predicate p) {
    return gmean(filter(std::forward<Array>(f), p));
}

template <class Array>
auto nangmean(Array &&f) {
    return gmean(std::forward<Array>(f), filters::not_nan);
}

//---------------------------------------------------------------------------------
// covariance
//---------------------------------------------------------------------------------

template <int ddof = 0, class InputIt1, class InputIt2, class Predicate>
constexpr auto covariance(InputIt1 first1,
                          InputIt1 last1,
                          InputIt2 first2,
                          InputIt2 last2,
                          Predicate filter) {
    using T1 = typename std::iterator_traits<InputIt1>::value_type;
    using T2 = typename std::iterator_traits<InputIt2>::value_type;
    using raw_t1 = units::representation_t<T1>;
    using raw_t2 = units::representation_t<T2>;
    using raw_t = std::common_type_t<raw_t1, raw_t2>;
    using prod_t = decltype(std::declval<T1>() * std::declval<T2>());

    static_assert(meta::is_predicate<Predicate, T1>);
    static_assert(meta::is_predicate<Predicate, T2>);

    scicpp_require(std::distance(first1, last1) ==
                   std::distance(first2, last2));

    if (std::distance(first1, last1) == 0) {
        return std::make_tuple(std::numeric_limits<prod_t>::quiet_NaN(),
                               signed_size_t(0));
    }

    // Pairwise recursive implementation of covariance summation
    const auto [m1_, m2_, cov_, c_] = pairwise_accumulate<64>(
        first1,
        last1,
        first2,
        last2,
        [&](auto f1, auto l1, auto f2, auto l2) {
            const auto m1 = mean(f1, l1, filter);
            const auto m2 = mean(f2, l2, filter);

            auto res = utils::set_zero<prod_t>();
            signed_size_t cnt = 0;

            for (; f1 != l1; ++f1, ++f2) {
                if (filter(*f1) && filter(*f2)) {
                    if constexpr (meta::is_complex_v<T2>) {
                        res += (*f1 - m1) * std::conj(*f2 - m2);
                    } else {
                        res += (*f1 - m1) * (*f2 - m2);
                    }
                    cnt++;
                }
            }

            return std::make_tuple(m1, m2, res, cnt);
        },
        [&](const auto res1, const auto res2) {
            // Combine covariances
            // https://stackoverflow.com/questions/45773857/merging-covariance-from-two-sets-to-create-new-covariance
            const auto [m11, m12, covar1, n1] = res1;
            const auto [m21, m22, covar2, n2] = res2;

            const auto n_c = n1 + n2;
            const auto m1_c = (raw_t1{1} / raw_t1(n_c)) *
                              (raw_t1(n1) * m11 + raw_t1(n2) * m21);
            const auto m2_c = (raw_t2{1} / raw_t2(n_c)) *
                              (raw_t2(n1) * m12 + raw_t2(n2) * m22);
            const auto covar_c = covar1 + covar2 +
                                 (raw_t(n1) * raw_t(n2) / raw_t(n_c)) *
                                     conj(m12 - m22) * (m11 - m21);
            return std::make_tuple(m1_c, m2_c, covar_c, n_c);
        });

    if (unlikely(c_ - ddof <= 0)) {
        return std::make_tuple(std::numeric_limits<decltype(cov_)>::infinity(),
                               c_);
    } else {
        return std::make_tuple(cov_ / raw_t(c_ - ddof), c_);
    }
}

template <int ddof = 0, class Array1, class Array2, class Predicate>
constexpr auto
covariance(const Array1 &f1, const Array2 &f2, Predicate filter) {
    return std::get<0>(covariance<ddof>(
        f1.cbegin(), f1.cend(), f2.cbegin(), f2.cend(), filter));
}

template <int ddof = 0, class Array1, class Array2>
constexpr auto covariance(const Array1 &f1, const Array2 &f2) {
    return covariance<ddof>(f1, f2, filters::all);
}

template <int ddof = 0, class Array1, class Array2>
auto nancovariance(const Array1 &f1, const Array2 &f2) {
    return covariance<ddof>(f1, f2, filters::not_nan);
}

//---------------------------------------------------------------------------------
// var
//---------------------------------------------------------------------------------

template <int ddof = 0, class InputIt, class Predicate>
constexpr auto var(InputIt first, InputIt last, Predicate filter) {
    const auto [v, n] = covariance<ddof>(first, last, first, last, filter);
    using T = std::decay_t<decltype(v)>;

    if constexpr (meta::is_complex_v<T>) {
        // The variance is always a nonnegative real number
        return std::make_tuple(std::real(v), n);
    } else {
        return std::make_tuple(v, n);
    }
}

template <int ddof = 0, class Array, class Predicate>
constexpr auto var(const Array &f, Predicate filter) {
    return std::get<0>(var<ddof>(f.cbegin(), f.cend(), filter));
}

template <int ddof = 0, class Array>
constexpr auto var(const Array &f) {
    return var<ddof>(f, filters::all);
}

template <int ddof = 0, class Array>
auto nanvar(const Array &f) {
    return var<ddof>(f, filters::not_nan);
}

template <int ddof = 1, class Array, typename T = typename Array::value_type>
constexpr auto tvar(const Array &f,
                    const std::array<T, 2> &limits,
                    const std::array<bool, 2> &inclusive = {true, true}) {
    return var<ddof>(f, filters::Trim<T>(limits, inclusive));
}

//---------------------------------------------------------------------------------
// std
//---------------------------------------------------------------------------------

template <int ddof = 0, class Array, class Predicate>
auto std(const Array &a, Predicate filter) {
    return units::sqrt(var<ddof>(a, filter));
}

template <int ddof = 0, class Array>
auto std(const Array &a) {
    return units::sqrt(var<ddof>(a));
}

template <int ddof = 0, class Array>
auto nanstd(const Array &a) {
    return units::sqrt(nanvar<ddof>(a));
}

template <int ddof = 1, class Array, typename T = typename Array::value_type>
auto tstd(const Array &a,
          const std::array<T, 2> &limits,
          const std::array<bool, 2> &inclusive = {true, true}) {
    return units::sqrt(tvar<ddof>(a, limits, inclusive));
}

//---------------------------------------------------------------------------------
// sem
//---------------------------------------------------------------------------------

template <int ddof = 1, class Array, class Predicate>
auto sem(const Array &a, Predicate filter) {
    const auto [v, n] = var<ddof>(a.cbegin(), a.cend(), filter);
    using T = std::decay_t<decltype(v)>;
    using raw_t = units::representation_t<T>;
    return units::sqrt(v / raw_t(n));
}

template <int ddof = 1, class Array>
auto sem(const Array &a) {
    const auto v = var<ddof>(a);
    using T = std::decay_t<decltype(v)>;
    using raw_t = units::representation_t<T>;
    return units::sqrt(v / raw_t(a.size()));
}

template <int ddof = 1, class Array>
auto nansem(const Array &a) {
    return sem<ddof>(a, filters::not_nan);
}

template <int ddof = 1, class Array, typename T = typename Array::value_type>
constexpr auto tsem(const Array &f,
                    const std::array<T, 2> &limits,
                    const std::array<bool, 2> &inclusive = {true, true}) {
    return sem<ddof>(f, filters::Trim<T>(limits, inclusive));
}

//---------------------------------------------------------------------------------
// moment
//---------------------------------------------------------------------------------

template <intmax_t n, class Array, class Predicate>
auto moment(const Array &f, [[maybe_unused]] Predicate filter) {
    using namespace operators;
    using T = typename Array::value_type;

    if constexpr (n == 0) {
        return T{1};
    } else if constexpr (n == 1) {
        return T{0};
    } else if constexpr (n == 2) {
        return var(f, filter);
    } else {
        // This allocates an extra array,
        // but preserves pairwise recursion precision
        return mean(pow<n>(f - mean(f, filter)), filter);
        // Combination of moments
        // http://prod.sandia.gov/techlib/access-control.cgi/2008/086212.pdf
    }
}

template <intmax_t n, class Array>
auto moment(const Array &f) {
    return moment<n>(f, filters::all);
}

template <intmax_t n, class Array>
auto nanmoment(const Array &f) {
    return moment<n>(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// kurtosis
//---------------------------------------------------------------------------------

enum KurtosisDef { Fisher = 0, Pearson = 1 };

template <KurtosisDef def = KurtosisDef::Fisher, class Array, class Predicate>
auto kurtosis(const Array &f, Predicate filter) {
    const auto m2 = moment<2>(f, filter);
    const auto m4 = moment<4>(f, filter);
    const auto k = m4 / (m2 * m2);

    if constexpr (def == KurtosisDef::Fisher) {
        using T = decltype(k);
        return k - T(3);
    } else {
        return k;
    }
}

template <KurtosisDef def = KurtosisDef::Fisher, class Array>
auto kurtosis(const Array &f) {
    return kurtosis<def>(f, filters::all);
}

template <KurtosisDef def = KurtosisDef::Fisher, class Array>
auto nankurtosis(const Array &f) {
    return kurtosis<def>(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// skew
//---------------------------------------------------------------------------------

template <class Array, class Predicate>
auto skew(const Array &f, Predicate filter) {
    const auto m2 = moment<2>(f, filter);
    const auto m3 = moment<3>(f, filter);
    return m3 / units::sqrt(m2 * m2 * m2);
}

template <class Array>
auto skew(const Array &f) {
    return skew(f, filters::all);
}

template <class Array>
auto nanskew(const Array &f) {
    return skew(f, filters::not_nan);
}

//---------------------------------------------------------------------------------
// covariance matrix
//---------------------------------------------------------------------------------

template <int ddof = 1, class Array1, class Array2, class Predicate>
auto cov(const Array1 &f1, const Array2 &f2, Predicate filter) {
    const auto covar = covariance<ddof>(f1, f2, filter);
    using T = std::decay_t<decltype(covar)>;

    Eigen::Matrix<T, 2, 2> res;
    res(0, 0) = T(var<ddof>(f1, filter));
    res(0, 1) = covar;
    res(1, 0) = conj(covar);
    res(1, 1) = T(var<ddof>(f2, filter));
    return res;
}

template <int ddof = 1, class Array1, class Array2>
auto cov(const Array1 &f1, const Array2 &f2) {
    return cov<ddof>(f1, f2, filters::all);
}

template <int ddof = 1, class Array1, class Array2>
auto nancov(const Array1 &f1, const Array2 &f2) {
    return cov<ddof>(f1, f2, filters::not_nan);
}

} // namespace scicpp::stats

#endif // SCICPP_CORE_STATS


#include <bits/stdc++.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/multiprecision/cpp_int.hpp>
using mpi = boost::multiprecision::int128_t;
using namespace boost::numeric;
using imatrix = ublas::matrix<mpi>;

using namespace std;
typedef long long int ll;
typedef pair<int, int> P;
//typedef pair<ll, ll> Pll;
typedef vector<int> Vi;
//typedef tuple<int, int, int> T;
#define FOR(i,s,x) for(int i=s;i<(int)(x);i++)
#define REP(i,x) FOR(i,0,x)
#define ALL(c) c.begin(), c.end()
#define DUMP( x ) cerr << #x << " = " << ( x ) << endl
#define UNIQUE(c) sort(ALL(c)), c.erase(unique(ALL(c)), c.end())

const int dr[4] = {-1, 0, 1, 0};
const int dc[4] = {0, 1, 0, -1};
const ll mod = (int)1e9 + 7;

void modulo(imatrix &mat, mpi mod) {
  for (size_t i = 0; i < mat.size1(); i++) {
    for (size_t j = 0; j < mat.size2(); j++) {
      mat(i, j) %= mod;
    }
  }
}

int main() {
  // use scanf in CodeForces!

  cin.tie(0);
  ios_base::sync_with_stdio(false);
  int N; ll K; cin >> N >> K;
  imatrix mat(N, N);
  REP(i, N) REP(j, N) cin >> mat(i, j);

  imatrix pow2 = mat, ans(N, N);
  REP(i, N) REP(j, N) ans(i, j) = (ll)(i == j);
  while (K) {
    if (K & 1LL) {
      ans = ublas::prod(ans, pow2);
      modulo(ans, mod);
    }
    pow2 = ublas::prod(pow2, pow2);
    modulo(pow2, mod);
    K >>= 1;
  }

  mpi count = 0;
  REP(i, N) REP(j, N) count = (count + ans(i, j)) % mod;
  cout << count << endl;
  return 0;
}


/**
 * @file
 * @brief Implementations from make_quad_rule.h
 * @author Raffael Casagrande
 * @date   2018-08-19 06:35:16
 * @copyright MIT License
 */

#include "make_quad_rule.h"
#include <Eigen/KroneckerProduct>
#include "gauss_quadrature.h"

namespace lf::quad {
namespace detail {
template <base::RefElType REF_EL, int Order>
QuadRule HardcodedQuadRule();
}

QuadRule make_QuadRule(base::RefEl ref_el, unsigned char order) {
  if (ref_el == base::RefEl::kSegment()) {
    quadOrder_t n = order / 2 + 1;
    auto [points, weights] = GaussLegendre(n);
    return QuadRule(base::RefEl::kSegment(), points.transpose(),
                    std::move(weights), 2 * n - 1);
  }
  if (ref_el == base::RefEl::kQuad()) {
    quadOrder_t n = order / 2 + 1;
    auto [points1d, weights1d] = GaussLegendre(n);
    Eigen::MatrixXd points2d(2, n * n);
    points2d.row(0) = Eigen::kroneckerProduct(points1d.transpose(),
                                              Eigen::MatrixXd::Ones(1, n));
    points2d.row(1) = points1d.transpose().replicate(1, n);
    return QuadRule(base::RefEl::kQuad(), std::move(points2d),
                    Eigen::kroneckerProduct(weights1d, weights1d), 2 * n - 1);
  }
  if (ref_el == base::RefEl::kTria()) {
    switch (order) {
      case 1:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 1>();
      case 2:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 2>();
      case 3:  // user order 4 rule instead
      case 4:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 4>();
      case 5:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 5>();
      case 6:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 6>();
      case 7:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 7>();
      case 8:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 8>();
      case 9:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 9>();
      case 10:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 10>();
      case 11:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 11>();
      case 12:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 12>();
      case 13:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 13>();
      case 14:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 14>();
      case 15:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 15>();
      case 16:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 16>();
      case 17:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 17>();
      case 18:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 18>();
      case 19:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 19>();
      case 20:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 20>();
      case 21:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 21>();
      case 22:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 22>();
      case 23:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 23>();
      case 24:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 24>();
      case 25:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 25>();
      case 26:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 26>();
      case 27:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 27>();
      case 28:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 28>();
      case 29:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 29>();
      case 30:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 30>();
      case 31:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 31>();
      case 32:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 32>();
      case 33:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 33>();
      case 34:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 34>();
      case 35:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 35>();
      case 36:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 36>();
      case 37:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 37>();
      case 38:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 38>();
      case 39:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 39>();
      case 40:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 40>();
      case 41:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 41>();
      case 42:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 42>();
      case 43:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 43>();
      case 44:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 44>();
      case 45:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 45>();
      case 46:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 46>();
      case 47:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 47>();
      case 48:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 48>();
      case 49:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 49>();
      case 50:
        return detail::HardcodedQuadRule<base::RefEl::kTria(), 50>();
      default:
        // Create a quadrule using tensor product quadrature rule + duffy
        // transform
        quadOrder_t n = order / 2 + 1;
        auto [leg_p, leg_w] = GaussLegendre(n);
        auto [jac_p, jac_w] = GaussJacobi(n, 1, 0);
        jac_p.array() = (jac_p.array() + 1) / 2.;  // rescale to [0,1]
        jac_w.array() *= 0.25;
        Eigen::MatrixXd points2d(2, n * n);
        points2d.row(0) = Eigen::kroneckerProduct(
            leg_p.transpose(), (1 - jac_p.transpose().array()).matrix());
        points2d.row(1) = jac_p.transpose().replicate(1, n);
        return QuadRule(base::RefEl::kTria(), std::move(points2d),
                        Eigen::kroneckerProduct(leg_w, jac_w), 2 * n - 1);
    }
  }
  LF_VERIFY_MSG(
      false, "No Quadrature rules implemented for this reference element yet.");
}
}  // namespace lf::quad


/*
 * Copyright 2018-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <Eigen/Core>
#include <eigen-osqp/CSCMatrix.h>
#include <eigen-osqp/OSQP.h>
#include <iostream>

int main()
{
    int nrVar = 6;
    int nrConstr = 5;
    auto inf = std::numeric_limits<double>::infinity();

    Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(nrVar, nrVar);
    Eigen::MatrixXd A(nrConstr, nrVar);
    A << 1., -1., 1., 0., 3., 1.,
        -1., 0., -3., -4., 5., 6.,
        2., 5., 3., 0., 1., 0.,
        0., 1., 0., 1., 2., -1.,
        -1., 0., 2., 1., 1., 0.;

    Eigen::VectorXd c(nrVar);
    Eigen::VectorXd AL(nrConstr);
    Eigen::VectorXd AU(nrConstr);
    Eigen::VectorXd XL(nrVar);
    Eigen::VectorXd XU(nrVar);
    c << 1., 2., 3., 4., 5., 6.;
    AL << 1., 2., 3., -inf, -inf;
    AU << 1., 2., 3., -1., 2.5;
    XL << -1000., -10000., 0., -1000., -1000., -1000.;
    XU << 10000., 100., 1.5, 100., 100., 1000.;

    Eigen::OSQP qp;

    qp.problem(nrVar, nrConstr);
    bool success = qp.solve(Q, c, A, AL, AU, XL, XU);

    Eigen::VectorXd result = qp.result();

    std::cout << "Problem:"
              << "\n\tminimize 0.5*x'*Q*x + c'*x"
              << "\n\twith     AL <= A <= AU"
              << "\n\t         XL <= x <= XU"
              << "\n\nQ:\n"
              << Q
              << "\nc:\n"
              << c.transpose()
              << "\nA:\n"
              << A
              << "\nAL:\n"
              << AL.transpose()
              << "\nAU:\n"
              << AU.transpose()
              << "\nXL:\n"
              << XL.transpose()
              << "\nXU:\n"
              << XU.transpose()
              << "\n\n\nSolution:\n"
              << result.transpose() << std::endl;

    std::cout << "Press enter to quit" << std::endl;
    std::cin.get();
}


// Copyright (c) 2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <drake/math/discrete_algebraic_riccati_equation.h>

namespace frc3512 {

/**
 * Returns solution to the DARE.
 *
 * @param A System matrix.
 * @param B Input matrix.
 * @param Q State cost matrix.
 * @param R Input cost matrix.
 * @param N State-input cross-term cost matrix.
 */
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
    const Eigen::Matrix<double, States, States>& A,
    const Eigen::Matrix<double, States, Inputs>& B,
    const Eigen::Matrix<double, States, States>& Q,
    const Eigen::Matrix<double, Inputs, Inputs>& R,
    const Eigen::Matrix<double, States, Inputs>& N) {
    Eigen::Matrix<double, States, States> scrA =
        A - B * R.llt().solve(N.transpose());
    Eigen::Matrix<double, States, States> scrQ =
        Q - N * R.llt().solve(N.transpose());

    return drake::math::DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);
}

}  // namespace frc3512


#include <Eigen/Dense>
#include <tuple>
#include <iostream>
#include <fstream>
#include <string>
#include "NARMAL2Network.h"
#include "util.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::ArrayXd;
using narmal2::NARMAL2Network;
using narmal2::writeCSV;

/**
 * Generate data for the example system
 * @param n number of samples to be produced
 * @return X and y
 */
std::tuple<MatrixXd, VectorXd> generateData(const VectorXd& u)
{
	int n = u.size();
	double y0 = 0.3, y_1 = 0, u_1 = 0; // intial values
	MatrixXd X{ n, 3 };
	VectorXd y{ n + 1 };
	// compute y(1) manually: [y(k), y(k-1), u(k-1)] -> y(k+1)
	y(0) = y0;
	X.row(0) << y0, y_1, u_1;
	y(1) = 1.5 * y0 * y_1 / (1 + y0 * y_1 + y_1*y_1)
		+ std::sin(y0 + y_1) + 0.8 * u_1 + u(0) * std::cos(y0 - u_1);

	for (int k = 1; k < n; k++)
	{
		X.row(k) << y(k), y(k - 1), u(k - 1);
		y(k + 1) = 1.5 * y(k) * y(k - 1) / (1 + y(k) * y(k) + y(k - 1)*y(k - 1))
			+ std::sin(y(k) + y(k - 1)) + 0.8 * u(k - 1) + std::cos(y(k) - u(k-1)) * u(k);
	}
	return { X, y.bottomRows(n) };
}

/**
 * Generate the training or test dataset.
 * @param which "train" or "test"
 * @return X, u, and y
 */
std::tuple<MatrixXd, VectorXd, VectorXd> generateDataSet(const std::string& which)
{
	assert(which == "train" || which == "test");
	if (which == "train")
	{
		// generate data for training set
		ArrayXd k;
		int n = 5000;
		MatrixXd X;
		VectorXd y;
		k.setLinSpaced(n, 1, n);
		Eigen::VectorXd u = (1 * 3.14 / 3 * k).sin() + (2 * 3.14 / 24 * k).sin()
			+ ArrayXd::Random(n) * 0.2;
		std::tie(X, y) = generateData(u);
		return { X, u, y };
	}
	else
	{
		// generate data for test set
		int ntest = 200;
		ArrayXd k;
		k.setLinSpaced(ntest, 1, ntest); // [1, 2, ..., ntest]
		VectorXd utest = (1 * 3.14 / 5 * k).cos() + (2 * 3.14 / 25 * k).sin()
			+ ArrayXd::Random(ntest) * 0.1;
		MatrixXd Xtest;
		VectorXd ytest;
		std::tie(Xtest, ytest) = generateData(utest);
		return { Xtest, utest, ytest };
	}
}

int main()
{
	// get training set
	MatrixXd X;
	VectorXd u, y;
	std::tie(X, u, y) = generateDataSet("train");

	// train a network: 3 inputs,
	// the upper subnet has 30 hidden neurons and the lower 20 hidden neurons.
	NARMAL2Network net{ 3, {40}, {30}, "ReLU" };
	int batchSize = 500;
	int nIterations = 3000;
	auto stats = net.trainBatch(X, u, y, 1e-5, batchSize, nIterations, "momentum",
		{ {"learningRate", 1e-2}, {"gamma", 0.9}, {"decayRate", 0.98} });
	auto yp = net.predict(X, y);

	// get the test set and make predictions
	MatrixXd Xtest;
	VectorXd utest, ytest;
	std::tie(Xtest, utest, ytest) = generateDataSet("test");
	auto ytestp = net.predict(Xtest, utest);

	// write results CSV files for further analysis using for example MATLAB
	writeCSV("trainingLoss.csv", stats.trainLossHistory);
	Eigen::MatrixXd testResults{ytest.rows(), 2 };
	testResults.col(0) = ytest;
	testResults.col(1) = ytestp;
	writeCSV("testResults.csv", testResults);
	Eigen::MatrixXd trainingResults{ yp.rows(), 2 };
	trainingResults.col(0) = y;
	trainingResults.col(1) = yp;
	writeCSV("trainingResults.csv", trainingResults);

	std::getchar();
}


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

using namespace std;
int main(int, char**)
{
    typedef double value_type;
    typedef int    size_type;
    const int nb_elements= 6, nb_nodes= 12;

    value_type array[][4]= {{2, 3,   4,   5},
			    {4, 10, 13,  16},
			    {6, 25, 38,  46},
			    {8, 32, 77, 100}};
    mtl::dense2D<value_type>   E_mat(array);

    std::cout<<"E_mat=\n"<< E_mat <<"\n";

    mtl::mat::element_structure<value_type> A;

    typedef mtl::mat::element<value_type>	element_type;
    element_type* elements = new element_type[nb_elements];

    mtl::dense_vector<size_type>  index_a(4, 0),
				   index_b(4, 0),
				   index_c(4, 0),
				   index_d(4, 0),
				   index_e(4, 0),
				   index_f(4, 0);

    // construct nodes for every element
    index_a[0]= 0; index_a[1]= 1; index_a[2]= 4; index_a[3]= 5;
    index_b= index_a + 1;
    index_c= index_a + 2;
    index_d= index_a + 4;
    index_e= index_a + 5;
    index_f= index_a + 6;

    //construct the 6 elements from the example grid
    element_type a(0, index_a, E_mat);
    element_type b(1, index_b, E_mat);
    element_type c(2, index_c, E_mat);
    element_type d(3, index_d, E_mat);
    element_type e(4, index_e, E_mat);
    element_type f(5, index_f, E_mat);

    //construct neighborhood information for each element
    a.add_neighbors(&b, &d, &e);
    b.add_neighbors(&a, &c, &d, &e, &f);
    c.add_neighbors(&a, &b, &e);
    d.add_neighbors(&a, &b, &e);
    e.add_neighbors(&a, &b, &c, &d, &f);
    f.add_neighbors(&b, &c, &e);

    std::cout<< "a=" << a << "\n";

    //construct array of elements
    elements[0]=a;
    elements[1]=b;
    elements[2]=c;
    elements[3]=d;
    elements[4]=e;
    elements[5]=f;

    //construct element_structure from the 6 single elements
    A.consume(nb_elements, nb_nodes, elements);
    mtl::dense_vector<value_type> x(nb_nodes, 1.0), test(A * x);

    std::cout<< "test="<< test << "\n";

    return 0;
}


 /*
 * functions.cpp
 *
 */
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <numeric>

#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/types.h"
#include "core/functions.h"

using namespace std;
using namespace yann;

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Activation functions implementation
//

// identity function:
//  f(x) = x
//  d f(x<i>) / d(x<j>) = 1 if i == j and 0 if i != j
string yann::IdentityFunction::get_info() const
{
  return "Identity";
}
void yann::IdentityFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  switch(mode) {
  case Operation_Assign:
    output.noalias() = input;
    break;
  case Operation_PlusEqual:
    output.noalias() += input;
    break;
  }
}
void yann::IdentityFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  output.setOnes();
}
unique_ptr<ActivationFunction> yann::IdentityFunction::copy() const
{
  return make_unique<IdentityFunction>();
}

// rectified linear unit function or leaky ReLU if a != 0:
//  f(x) = x for x > 0 ; a*x for x < 0
//  d f(x<i>) / d(x<j>) = 1 if x > 0; a if x < 0
string yann::ReluFunction::get_info() const
{
  return "Relu";
}
void yann::ReluFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  switch(mode) {
  case Operation_Assign:
    output.setZero();
    break;
  case Operation_PlusEqual:
    // do nothing
    break;
  }

  const auto batch_size = get_batch_size(output);
  const auto batch_item_size = get_batch_item_size(output);
  for(MatrixSize ii = 0 ; ii < batch_size; ++ii) {
    for(MatrixSize jj = 0 ; jj < batch_item_size; ++jj) {
      if(get_batch(input, ii)(jj) >= 0) {
        get_batch(output, ii)(jj) += get_batch(input, ii)(jj);
      } else {
        get_batch(output, ii)(jj) += _a * get_batch(input, ii)(jj);
      }
    }
  }
}
void yann::ReluFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  const auto batch_size = get_batch_size(output);
  const auto batch_item_size = get_batch_item_size(output);
  for(MatrixSize ii = 0 ; ii < batch_size; ++ii) {
    for(MatrixSize jj = 0 ; jj < batch_item_size; ++jj) {
      if(get_batch(input, ii)(jj) >= 0) {
        get_batch(output, ii)(jj) = 1;
      } else {
        get_batch(output, ii)(jj) = _a;
      }
    }
  }
}
unique_ptr<ActivationFunction> yann::ReluFunction::copy() const
{
  return make_unique<ReluFunction>(_a);
}

// sigmoid function:
//  f(x) = 1 / (1 + exp(-x))
string yann::SigmoidFunction::get_info() const
{
  return "Sigmoid";
}
Value yann::SigmoidFunction::sigmoid_scalar(const Value & x)
{
  return 1 / (1 + exp(-x));
}

Value yann::SigmoidFunction::sigmoid_derivative_scalar(const Value & x)
{
  Value s = sigmoid_scalar(x);
  return s * (1 - s);
}

void yann::SigmoidFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  switch(mode) {
  case Operation_Assign:
    output.array() = 1 / (1 + exp(-input.array()));
    break;
  case Operation_PlusEqual:
    output.array() += 1 / (1 + exp(-input.array()));
    break;
  }
}

void yann::SigmoidFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  this->f(input, output);
  output.array() =  output.array() * (1 -  output.array());
}
unique_ptr<ActivationFunction> yann::SigmoidFunction::copy() const
{
  return make_unique<SigmoidFunction>();
}

// Fast sigmoid function:
//  f(x) = 1 / (1 + exp(-x))
yann::FastSigmoidFunction::FastSigmoidFunction(const size_t & table_size, const Value & max_value) :
    _table(table_size + 1),
    _max_value(max_value)
{
  // populates _table with sigmoid(x) values for x in (-max_value, max_value)
  for(size_t ii = 0; ii <= table_size; ++ii) {
    _table[ii] = SigmoidFunction::sigmoid_scalar((2 * ii / (Value)table_size - 1) * max_value);
  }
}

yann::FastSigmoidFunction::FastSigmoidFunction(const SigmoidTable & table, const Value & max_value) :
    _table(table),
    _max_value(max_value)
{
}

string yann::FastSigmoidFunction::get_info() const
{
  ostringstream oss;
  oss << "FastSigmoidFunction["
      << "max_value=" << _max_value
      << ", table_size=" << _table.size()
      << "]"
      ;
  return oss.str();
}

void yann::FastSigmoidFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  auto sigmoid = [&](const Value & xx) -> Value {
    if(xx <= -_max_value) {
      return 0;
    } else if(xx >= _max_value) {
      return 1;
    }
    auto index = (size_t)((xx + _max_value) * (_table.size() - 1) / (2 * _max_value));
    YANN_SLOW_CHECK_GE(index, 0);
    YANN_SLOW_CHECK_LT(index, _table.size());
    return _table[index];
  };

  switch(mode) {
  case Operation_Assign:
    for(MatrixSize ii = 0; ii < input.rows(); ++ii) {
      for(MatrixSize jj = 0; jj < input.cols(); ++jj) {
        output(ii, jj) = sigmoid(input(ii, jj));
      }
    }
    break;
  case Operation_PlusEqual:
    for(MatrixSize ii = 0; ii < input.rows(); ++ii) {
      for(MatrixSize jj = 0; jj < input.cols(); ++jj) {
        output(ii, jj) += sigmoid(input(ii, jj));
      }
    }
    break;
  }
}

void yann::FastSigmoidFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  this->f(input, output);
  output.array() =  output.array() * (1 -  output.array());
}
unique_ptr<ActivationFunction> yann::FastSigmoidFunction::copy() const
{
  // can't use make_unique<> because this constructor is private
  auto res = new FastSigmoidFunction(_table, _max_value);
  return unique_ptr<ActivationFunction>(res);
}

// tanh function:
//  f(x) = A * tanh(S*x)
//  df/dx = A * S * (1−(tanh(S*x))^2)
string yann::TanhFunction::get_info() const
{
  ostringstream oss;
  oss << "Tanh["
      << "A=" << _AA
      << ", S=" << _SS
      << "]"
      ;
  return oss.str();
}

void yann::TanhFunction::f(const RefConstVectorBatch & input, RefVectorBatch output, enum OperationMode mode)
{
  YANN_SLOW_CHECK(is_same_size(input, output));

  switch(mode) {
  case Operation_Assign:
    output.array() = _AA * tanh(input.array() * _SS);
    break;
  case Operation_PlusEqual:
    output.array() += _AA * tanh(input.array()* _SS);
    break;
  }
}

void yann::TanhFunction::derivative(const RefConstVectorBatch & input, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(input, output));
  this->f(input, output);
  output.array() = (1 - tanh(input.array()* _SS).square()) * (_AA * _SS);
}
unique_ptr<ActivationFunction> yann::TanhFunction::copy() const
{
  return make_unique<TanhFunction>(_AA, _SS);
}

// quadratic cost function:
//  f(actual, expected) = sum((actual<i> - expected<i>)^2)
//  d f(actual, expected) / d (actual) = (actual<i> - expected<i>)
string yann::QuadraticCost::get_info() const
{
  return "Quadratic";
}
Value yann::QuadraticCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  return ((actual.array() - expected.array()).square()).sum();
}
void yann::QuadraticCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));
  output.noalias() = 2 * (actual - expected);
}
unique_ptr<CostFunction> yann::QuadraticCost::copy() const
{
  return make_unique<QuadraticCost>();
}

// Exponential cost:
//  f(actual, expected) = tau * exp(sum((actual<i> - expected<i>)^2) / tau)
//  d f(actual, expected) / d (actual) = 2 * f(actual, expected) * (actual<i> - expected<i>)
string yann::ExponentialCost::get_info() const
{
  ostringstream oss;
  oss << "Exponential["
      << "tau=" << _tau
      << "]"
      ;
  return oss.str();
}
Value yann::ExponentialCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  const auto val = (actual.array() - expected.array()).square().sum();
  return _tau * exp(val / _tau);
}
void yann::ExponentialCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));
  output.noalias() = (2 * f(actual, expected)) * (actual - expected);
}
unique_ptr<CostFunction> yann::ExponentialCost::copy() const
{
  return make_unique<ExponentialCost>(_tau);
}

// cross entropy function:
//  f(actual, expected) = sum(-(expected * ln(actual) + (1 - expected) * ln(1 - actual)))
//  d f(actual, expected) / d (actual) = (actual - expected) / ((1 - actual) * actual)
string yann::CrossEntropyCost::get_info() const
{
  ostringstream oss;
  oss << "CrossEntropy["
      << "epsilon=" << _epsilon
      << "]"
      ;
  return oss.str();
}

Value yann::CrossEntropyCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));

  // iterate over elements manually and use small _epsilon to avoid hitting nan
  Value res = 0;
  for(MatrixSize ii = 0; ii < actual.rows(); ++ii) {
    for(MatrixSize jj = 0; jj < actual.cols(); ++jj) {
      auto aa = actual(ii, jj);
      auto ee = expected(ii, jj);
      if(aa <= 0) {
        aa = _epsilon;
      } else if(aa >= 1) {
        aa = 1 - _epsilon;
      }
      res += (ee * log(aa) + (1 - ee) * log(1 - aa));
    }
  }
  return -(res);
}

void yann::CrossEntropyCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));

  // iterate over elements manually and use small _epsilon to avoid hitting nan
  for(MatrixSize ii = 0; ii < actual.rows(); ++ii) {
    for(MatrixSize jj = 0; jj < actual.cols(); ++jj) {
      auto aa = actual(ii, jj);
      auto ee = expected(ii, jj);
      if(fabs(aa - ee) > _epsilon) {
        if(aa <= 0) {
          aa = _epsilon;
        } else if(aa >= 1) {
          aa = 1 - _epsilon;
        }
        output(ii, jj) = (aa - ee) / ((1 - aa) * aa);
      } else {
        output(ii, jj) = 0;
      }
    }
  }
}

unique_ptr<CostFunction> yann::CrossEntropyCost::copy() const
{
  return make_unique<CrossEntropyCost>(_epsilon);
}

// Hellinger distance cost:
//  f(actual, expected) = sum(sqrt(actual) - sqrt(expected))^2
//  d f(actual, expected) / d (actual) =  (1 -  sqrt(expected) /  sqrt(actual))
string yann::HellingerDistanceCost::get_info() const
{
  ostringstream oss;
  oss << "HellingerDistance["
      << "epsilon=" << _epsilon
      << "]"
      ;
  return oss.str();
}

Value yann::HellingerDistanceCost::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  return (((actual.array() + _epsilon).sqrt() - expected.array().sqrt()).square()).sum();
}

void yann::HellingerDistanceCost::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));
  output.array() = (1 - expected.array().sqrt() / (actual.array() + _epsilon).sqrt());
}
unique_ptr<CostFunction> yann::HellingerDistanceCost::copy() const
{
  return make_unique<HellingerDistanceCost>(_epsilon);
}

// Squared Hinge Loss:
//  f(actual, expected) = (max(0, 1 - actual * expected))^2
//  d f(actual, expected) / d (actual(i)) =  if (actual * expected) > 1 then 0; otherwise -2 * (1 - actual * expected) * expected(i)
string yann::SquaredHingeLoss::get_info() const
{
  return "SquaredHingeLoss";
}
Value yann::SquaredHingeLoss::f(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));

  Value res = 0;
  for(MatrixSize ii = 0; ii < get_batch_size(actual); ++ii) {
    const auto aa = get_batch(actual, ii);
    const auto ee = get_batch(expected, ii);

    const Value vv = (aa.array() * ee.array()).sum();
    if(vv < 1.0) {
      res += (1.0 - vv) * (1.0 - vv);
    }
  }
  return res;
}

void yann::SquaredHingeLoss::derivative(const RefConstVectorBatch & actual, const RefConstVectorBatch & expected, RefVectorBatch output)
{
  YANN_SLOW_CHECK(is_same_size(actual, expected));
  YANN_SLOW_CHECK(is_same_size(actual, output));

  for(MatrixSize ii = 0; ii < get_batch_size(actual); ++ii) {
    const auto aa = get_batch(actual, ii);
    const auto ee = get_batch(expected, ii);
    auto oo = get_batch(output, ii);

    const Value vv = (aa.array() * ee.array()).sum();
    if(vv < 1) {
      oo = - 2 * (1.0 - vv) * ee;
    } else {
      oo.setZero();
    }
  }
}

unique_ptr<CostFunction> yann::SquaredHingeLoss::copy() const
{
  return make_unique<SquaredHingeLoss>();
}


#include <Eigen/Dense>
#include <string.h>
#include <fstream>
#include <iostream>
using namespace std;

#define MODEL_PATH_DISTANCE 100
#define POLYFIT_DEGREE 4

Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;

void poly_fit(float *in_pts, float *in_stds, float *out) {
  // References to inputs
  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > pts(in_pts, MODEL_PATH_DISTANCE);
  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > std(in_stds, MODEL_PATH_DISTANCE);
  Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE, 1> > p(out, POLYFIT_DEGREE);

  // Build Least Squares equations
  Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> lhs = vander.array().colwise() / std.array();
  Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();

  // Improve numerical stability
  Eigen::Matrix<float, POLYFIT_DEGREE, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
  lhs = lhs * scale.asDiagonal();

  // Solve inplace
  Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
  p = qr.solve(rhs);

  // Apply scale to output
  p = p.transpose() * scale.asDiagonal();
}

int main(int argc, char** argv)
{
	for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
    for(int j = 0; j < POLYFIT_DEGREE; j++) {
      vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
    }
  }
  float* points = new float[MODEL_PATH_DISTANCE]();
  float* stds = new float[MODEL_PATH_DISTANCE]();
  float* poly = new float[POLYFIT_DEGREE]();
  string filename = argv[1];
  string name = filename + "_pts";
  fstream file(name.c_str());
  float tp;
  int idx=0;
  while(file >> tp)
  {
    // cout << tp << " ";
    points[idx] = tp;
    idx++;
  }
  cout << endl;
  file.close();
  name = filename+"_stds";
  fstream file2(name.c_str());
  idx=0;
  while(file2 >> tp)
  {
    // cout << tp << " ";
    stds[idx] = tp;
    idx++;
  }

  poly_fit(points, stds, poly);
  cout << poly[0];
  for (int i=1;i<POLYFIT_DEGREE;i++)
  {
    cout << " " << poly[i];
  }
  cout << endl;
	return 0;
}


/*
 * Author: Mario Lüder
 * Date: Nov. 2018
 */

#ifndef MOTIONUKF_HPP
#define MOTIONUKF_HPP

#include "Ukf.hpp"
#include "tools.h"

#include <Eigen/Dense>
#include <sstream>

class ProcessModel
{
public:
   struct MotionParams
   {
      double p_x;
      double p_y;
      double v;
      double yaw;
      double yaw_dot;
      double std_a;
      double std_yaw_dot_dot;
   };

   static inline void normalize(const long colIdx, Eigen::MatrixXd &XSigmaPointsDiff)
   {
      double & yaw = XSigmaPointsDiff.col(colIdx)(3);

      // normalize phi to -/+ pi
      Tools::normalizeAngle(yaw);
   }

   static void predictSigmaPoints(const Eigen::MatrixXd & XSigmaPoints, const double d_t_s, Eigen::MatrixXd & XSigmaPointsPred);
   static inline void processModel(const MotionParams & x, const double d_t_s, MotionParams & x_pred);

   //set state dimension
   static constexpr long m_n_x = 5;

   //set augumented state dimension
   static constexpr long m_n_x_aug = 7;

   // constant 2 * PI
   static constexpr double TWO_PI = 2 * M_PI;

   static constexpr long m_nSigmaPoints = 1 + 2 * m_n_x_aug;

   // Process noise standard deviation longitudinal acceleration in m/s^2
   // static constexpr double m_std_a = 0.2;
   static constexpr double m_std_a = 1.5;

   // Process noise standard deviation yaw acceleration in rad/s^2
   // static constexpr double m_std_yawdd = 0.2;
   static constexpr double m_std_yawdd = 1;
};

class MotionUkf : public Ukf<ProcessModel>
{
public:
   MotionUkf()
      : Ukf<ProcessModel>()
   {}

   virtual ~MotionUkf();

   struct Measurement
   {
      Measurement(int32_t dim) : value(dim) {}
      Eigen::VectorXd value;
   };

   struct RadarMeasurement : public Measurement
   {
      RadarMeasurement() : Measurement(3) {}
      RadarMeasurement(std::istringstream & iss)  : Measurement(3)
      {
         double rho;
         double phi;
         double rho_dot;
         iss >> rho;
         iss >> phi;
         iss >> rho_dot;

         value << rho, phi, rho_dot;
      }

      RadarMeasurement & operator=(std::istringstream & iss)
      {
         double rho;
         double phi;
         double rho_dot;
         iss >> rho;
         iss >> phi;
         iss >> rho_dot;

         value << rho, phi, rho_dot;
         return  *this;
      }

      double & rho() {return value(0);}
      double & phi() {return value(1);}
      double & rho_dot() {return value(2);}

      struct Definition
      {
         double rho;
         double phi;
         double rho_dot;
      };

      static Definition * cast(void * data)
      {
         return reinterpret_cast<Definition*>(data);
      }
   };

   struct RadarMeasurementModel
   {
      using MEASUREMENT_T = RadarMeasurement;

      RadarMeasurementModel();

      static inline void normalize(const long colIdx, Eigen::MatrixXd &ZSigmaPointsDiff)
      {
         //double & phi = reinterpret_cast<RadarMeasurement*>(ZSigmaPointsDiff.col(colIdx).data())->phi;
         double & phi = RadarMeasurement::cast(ZSigmaPointsDiff.col(colIdx).data())->phi;

         // normalize phi to -/+ pi
         Tools::normalizeAngle(phi);
      }

      static inline void normalize(Eigen::MatrixXd &matrix)
      {
         const int cols = matrix.cols();

         for (int colIdx = 0; colIdx < cols; ++colIdx)
         {
            normalize(colIdx, matrix);
         }
      }

      static inline void normalize(Eigen::VectorXd &vector)
      {
         double & phi = RadarMeasurement::cast(vector.data())->phi;

         // normalize phi to -/+ pi
         Tools::normalizeAngle(phi);
      }

      void predictCovar(const Ukf<ProcessModel> & ukf, const Eigen::MatrixXd & XSigmaPoints, const Eigen::VectorXd & x_pred, Eigen::MatrixXd & P_pred) const;
      void predictMeasurement(const Eigen::MatrixXd & XSigmaPointPred, Eigen::MatrixXd & ZSigmaPointsRadarPred) const;

      static constexpr long nRadarValues = 3;

      //radar measurement noise standard deviation radius in m
      const double m_std_radr = 0.3;

      //radar measurement noise standard deviation angle in rad
      const double m_std_radphi = 0.03;

      //radar measurement noise standard deviation radius change in m/s
      const double m_std_radrd = 0.3;

      MEASUREMENT_T measurement;

      // NIS
      double m_nis = 0.0;

   private:
      Eigen::MatrixXd m_R;
   };

   struct LaserMeasurement : public Measurement
   {
      LaserMeasurement() : Measurement(2) {}
      LaserMeasurement(std::istringstream & iss) : Measurement(2)
      {
         double px;
         double py;
         iss >> px;
         iss >> py;
         value << px , py;
      }

      LaserMeasurement & operator=(std::istringstream & iss)
      {
         double px;
         double py;
         iss >> px;
         iss >> py;
         value << px, py;
         return *this;
      }

      double & px() {return value(0);}
      double & py() {return value(1);}

      struct Definition
      {
         double px;
         double py;
      };

      static Definition * cast(void * data)
      {
         return reinterpret_cast<Definition*>(data);
      }
   };

   struct LaserMeasurementModel
   {
      using MEASUREMENT_T = LaserMeasurement;

      LaserMeasurementModel();

      static inline void normalize(const long colIdx, Eigen::MatrixXd &ZSigmaPointsDiff)
      {
         (void)colIdx;
         (void)ZSigmaPointsDiff;
         // nothing to normalize
      }

      static inline void normalize(Eigen::MatrixXd &matrix)
      {
         (void)matrix;
      }

      static inline void normalize(Eigen::VectorXd &vector)
      {
         (void)vector;
      }

      void predictCovar(const Ukf<ProcessModel> & ukf, const Eigen::MatrixXd & XSigmaPoints, const Eigen::VectorXd & x_pred, Eigen::MatrixXd & P_pred) const;
      void predictMeasurement(const Eigen::MatrixXd & XSigmaPointPred, Eigen::MatrixXd & ZSigmaPointsLaserPred) const;

      // Process noise standard deviation longitudinal acceleration in m/s^2
      double m_std_a = 30;

      //laser measurement noise standard deviation (position) in m
      const double m_std_laser_pos = 0.15;

      static constexpr long nLaserValues = 2;

      MEASUREMENT_T measurement;

      // NIS
      double m_nis = 0.0;
   private:

      // Laser Measurement Covariance Matrix
      Eigen::MatrixXd m_L;
   };

   template<typename MEASUREMENTMODEL_T>
   void processMeasurement(MEASUREMENTMODEL_T & measurementModel, int64_t timestamp_us)
   {
      Ukf<ProcessModel>::processMeasurement<typename MEASUREMENTMODEL_T::MEASUREMENT_T, MEASUREMENTMODEL_T>(measurementModel.measurement, measurementModel, timestamp_us);
   }

   void init(const LaserMeasurementModel & laserMeasurementModel, int64_t timestamp_us)
   {
      Ukf<ProcessModel>::setNewTime(timestamp_us);
      m_x.setZero(ProcessModel::m_n_x);

      // set initial x pos
      m_x(0) = laserMeasurementModel.measurement.value(0);

      // set initial y pos
      m_x(1) = laserMeasurementModel.measurement.value(1);

      // initialize process noise
      m_std.setZero(2);
      m_std(0) = ProcessModel::m_std_a;
      m_std(1) = ProcessModel::m_std_yawdd;
   }

   RadarMeasurementModel radarMeasurementModel;
   LaserMeasurementModel laserMeasurementModel;
};

#endif // MOTIONUKF_HPP


#include <triumf/nmr/dipole_dipole.hpp>

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

#include <cmath>

double dipole_dipole(const double *x, const double *par) {
  double omega_d = std::abs(par[2] * par[3] * par[0] * par[0]);
  double factor = par[1] * (3.0 / 10.0);
  double normalization = factor / omega_d;
  return normalization * triumf::nmr::dipole_dipole::slr_rate<double>(
                             *x, par[0], par[1], par[2], par[3]);
}

double generic(const double *x, const double *par) {
  double B_d = par[0];
  double nu_c = par[1];
  double gamma_I = par[2];
  double omega = gamma_I * x[0];
  double omega_d = gamma_I * gamma_I * B_d * B_d;
  return triumf::nmr::dipole_dipole::j<double>(omega, nu_c) * par[1];
}

void plot_dipole_dipole() {

  const double B_min = 2e-5;
  const double B_max = 2e-1;

  const auto n_points = 200;

  const double B_d = 1e-5;
  const double nu_c = 1.0 / 23.8e-6;
  const double gamma_8Li = boost::math::constants::two_pi<double>() * 6.30221e6;
  const double gamma_93Nb =
      boost::math::constants::two_pi<double>() * 10.30221e6;

  TCanvas *canvas = new TCanvas();

  TF1 *f_dipole = new TF1("f_dipole", dipole_dipole, B_min, B_max, 4);
  f_dipole->SetTitle("");
  f_dipole->SetNpx(n_points);
  f_dipole->SetLineColor(kRed);
  f_dipole->SetParameter(0, B_d);
  f_dipole->SetParameter(1, nu_c);
  f_dipole->SetParameter(2, gamma_8Li);
  f_dipole->SetParameter(3, gamma_93Nb);

  f_dipole->GetHistogram()->GetXaxis()->SetTitle("B_{0} (T)");
  f_dipole->GetHistogram()->GetYaxis()->SetTitle(
      "[1/T_{1}(B_{0})] / [1/T_{1}(0)]");

  f_dipole->Draw();

  TF1 *f_generic = new TF1("f_generic", generic, B_min, B_max, 3);
  f_generic->SetTitle("");
  f_generic->SetLineColor(kBlue);
  f_generic->SetNpx(n_points);
  f_generic->SetParameter(0, B_d);
  f_generic->SetParameter(1, nu_c);
  f_generic->SetParameter(2, gamma_8Li);

  f_generic->GetHistogram()->GetXaxis()->SetTitle("B_{0} (T)");
  f_generic->GetHistogram()->GetYaxis()->SetTitle(
      "[1/T_{1}(B_{0})] / [1/T_{1}(0)]");

  f_generic->Draw("same");

  auto legend = new TLegend(0.15, 0.15, 0.60, 0.45);
  // option "C" allows to center the header
  legend->SetHeader("Models for dipole-dipole SLR 1/T_{1}", "C");
  legend->AddEntry(f_dipole, "Eq. (8.21) in Mehring (1983)", "l");
  legend->AddEntry(f_generic, "Generic BPP expression", "l");
  legend->Draw();

  // logarithmic scale
  gPad->SetLogx();
  gPad->SetLogy();

  // tick marks on all sides of the plot
  gPad->SetTickx();
  gPad->SetTicky();

  // grid lines
  gPad->SetGridx();
  gPad->SetGridy();

  canvas->Print("dipole-dipole.pdf", "EmbedFonts");
}


#include "geometrycentral/numerical/linear_solvers.h"
#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/simple_polygon_mesh.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include <emscripten/bind.h>
#include <emscripten/val.h>

#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/simple_polygon_mesh.h"

#include "interpolate_vectors.h"

using namespace emscripten;
using namespace geometrycentral;
using namespace geometrycentral::surface;

struct GeoMesh {
    std::unique_ptr<ManifoldSurfaceMesh> mesh;
    std::unique_ptr<VertexPositionGeometry> geom;
};

std::vector<Vector3> toList(ManifoldSurfaceMesh& mesh,
                            const VertexData<Vector3>& data) {
    std::vector<Vector3> vectorList;
    for (Vertex v : mesh.vertices()) {
        vectorList.push_back(data[v]);
    }
    return vectorList;
}

VertexData<Vector3> toData(ManifoldSurfaceMesh& mesh,
                           const std::vector<Vector3>& list) {
    VertexData<Vector3> data(mesh);
    for (size_t iV = 0; iV < mesh.nVertices(); iV++) {
        data[iV] = list[iV];
    }
    return data;
}

std::vector<Vector3> generateSmoothBoundaryField(GeoMesh& geo) {
    VertexData<Vector3> bField =
        generateSmoothBoundaryVectorField(*geo.mesh, *geo.geom);
    return toList(*geo.mesh, bField);
}

std::vector<Vector3> generateWavyBoundaryField(GeoMesh& geo, size_t frequency) {
    VertexData<Vector3> bField =
        generateWavyBoundaryVectorField(*geo.mesh, *geo.geom, frequency);
    return toList(*geo.mesh, bField);
}

std::vector<Vector3>
interpolateHarmonicFunction(GeoMesh& geo,
                            const std::vector<Vector3>& boundaryData) {
    return toList(*geo.mesh,
                  interpolateByHarmonicFunction(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData)));
}

std::vector<Vector3>
interpolateConnectionLaplacian(GeoMesh& geo,
                               const std::vector<Vector3>& boundaryData,
                               bool estimateNormalDirection) {
    return toList(*geo.mesh,
                  interpolateByConnectionLaplacian(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData),
                      estimateNormalDirection));
}

std::vector<Vector3>
interpolateStereographicProjection(GeoMesh& geo,
                                   const std::vector<Vector3>& boundaryData) {
    return toList(*geo.mesh,
                  interpolateByStereographicProjection(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData)));
}

std::vector<Vector3>
interpolateHarmonicMapToSphere(GeoMesh& geo,
                               const std::vector<Vector3>& boundaryData) {
    return toList(*geo.mesh,
                  interpolateByHarmonicMapToSphere(
                      *geo.mesh, *geo.geom, toData(*geo.mesh, boundaryData)));
}

// Stolen from Ricky Reusser https://observablehq.com/d/d0df0c04ce5c94FCC
template <typename T>
void copyToVector(const val& typedArray, std::vector<T>& vec) {
    unsigned int length = typedArray["length"].as<unsigned int>();
    val memory          = val::module_property("buffer");
    vec.reserve(length);
    val memoryView = typedArray["constructor"].new_(
        memory, reinterpret_cast<uintptr_t>(vec.data()), length);
    memoryView.call<void>("set", typedArray);
}

// Mostly stolen from Ricky Reusser https://observablehq.com/d/d0df0c04ce5c94fc
EMSCRIPTEN_BINDINGS(my_module) {
    value_array<Vector3>("Vector3")
        .element(&Vector3::x)
        .element(&Vector3::y)
        .element(&Vector3::z);
    value_array<Vector2>("Vector2").element(&Vector2::x).element(&Vector2::y);

    register_vector<Vector3>("VectorVector3");
    register_vector<size_t>("VectorSizeT");
    register_vector<std::vector<size_t>>("VectorVectorSizeT");

    class_<GeoMesh>("GCMesh")
        .function("polygons", optional_override([](GeoMesh& self) {
                      return self.mesh->getFaceVertexList();
                  }))
        .function("vertexCoordinates",
                  optional_override([](const GeoMesh& self) {
                      std::vector<Vector3> vCoords;
                      for (Vertex v : self.mesh->vertices())
                          vCoords.push_back(self.geom->inputVertexPositions[v]);
                      return vCoords;
                  }));

    function("readMesh",
             optional_override([](std::string str, std::string type = "") {
                 std::stringstream in;
                 in << str;

                 GeoMesh gMesh;
                 std::tie(gMesh.mesh, gMesh.geom) =
                     readManifoldSurfaceMesh(in, type);
                 return gMesh;
             }));

    function("generateSmoothBoundaryField", &generateSmoothBoundaryField);
    function("generateWavyBoundaryField", &generateWavyBoundaryField);

    function("interpolateHarmonicFunction", &interpolateHarmonicFunction);
    function("interpolateConnectionLaplacian", &interpolateConnectionLaplacian);
    function("interpolateStereographicProjection",
             &interpolateStereographicProjection);
    function("interpolateHarmonicMapToSphere", &interpolateHarmonicMapToSphere);
}


#pragma once

#include "../linesegment.hh"
#include "../../../util/Maybe.hh"

#include <Eigen/Core>

namespace bold
{
  template<typename T>
  class LineSegment2 : public LineSegment<T, 2>
  {
  public:
    typedef Eigen::Matrix<T,2,1> Point;

    LineSegment2(Point const& p1, Point const& p2)
    : LineSegment<T, 2>::LineSegment(p1, p2)
    {}

    LineSegment2(T x1, T y1, T x2, T y2)
    : LineSegment<T, 2>::LineSegment(Point(x1, y1), Point(x2, y2))
    {}

    using LineSegment<T, 2>::LineSegment;
    using LineSegment<T, 2>::operator=;

    Maybe<Point> tryIntersect(LineSegment2<T> const& other) const
    {
      double t;
      double u;
      return tryIntersect(other, t, u);
    }

    /**
    * Attempt to intersect two line segments.
    * Note that even if the line segments do not intersect, the t and u values will be set.
    * @param other the line to attempt intersection of this line with
    * @param t (output) the distance along this line at which intersection would occur, or NaN if lines are collinear/parallel
    * @param u (output) the distance along the other line at which intersection would occur, or NaN if lines are collinear/parallel
    * @return The point of intersection if within the line segments, or empty.
    */
    Maybe<Point> tryIntersect(LineSegment2<T> const& other, double& t, double& u) const
    {
      // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect

      Eigen::Vector2d pos1 = this->p1().template cast<double>();    // p
      Eigen::Vector2d pos2 = other.p1().template cast<double>();    // q
      Eigen::Vector2d dir1 = this->delta().template cast<double>(); // r
      Eigen::Vector2d dir2 = other.delta().template cast<double>(); // s

      // t = (q − p) × s / (r × s)
      // u = (q − p) × r / (r × s)

      double denom = fake2dCross(dir1, dir2);

      if (denom == 0)
      {
        // lines are collinear or parallel
        t = std::numeric_limits<double>::quiet_NaN();
        u = std::numeric_limits<double>::quiet_NaN();
        return Maybe<Point>::empty();
      }

      double t_numer = fake2dCross(pos2 - pos1, dir2);
      double u_numer = fake2dCross(pos2 - pos1, dir1);

      t = t_numer / denom;
      u = u_numer / denom;

      if (t < 0 || t > 1 || u < 0 || u > 1)
      {
        // line segments do not intersect within their ranges
        return Maybe<Point>::empty();
      }

      Eigen::Vector2d intersectionPoint = pos1 + dir1 * t;

      // If we are using integers, be sure to round the result before casting
      if (std::is_same<T,int>())
      {
        Eigen::Vector2d rounded = intersectionPoint.unaryExpr([](double v) { return round(v); });
        return Maybe<Point>(rounded.cast<T>());
      }

      return Maybe<Point>(intersectionPoint.cast<T>());
    }

  private:
    /**
     * Returns the magnitude of the vector that would result from a regular
     * 3D cross product of the input vectors, taking their Z values implicitly
     * as 0 (i.e. treating the 2D space as a plane in the 3D space)
     */
    inline static double fake2dCross(Eigen::Vector2d const& a, Eigen::Vector2d const& b)
    {
      return a.x()*b.y() - a.y()*b.x();
    }
  };

  typedef LineSegment2<double> LineSegment2d;
  typedef LineSegment2<float> LineSegment2f;
}


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

using namespace std;

int main()
{
  Eigen::Matrix4f m;
  m << 1, 2, 3, 4,
       5, 6, 7, 8,
       9, 10,11,12,
       13,14,15,16;
  cout << "m.leftCols(2) =" << endl << m.leftCols(2) << endl << endl;
  cout << "m.bottomRows<2>() =" << endl << m.bottomRows<2>() << endl << endl;
  m.topLeftCorner(1,3) = m.bottomRightCorner(3,1).transpose();
  cout << "After assignment, m = " << endl << m << endl;
}


/* Daniel R. Reynolds
   SMU Mathematics
   7 August 2020 */

// Inclusions
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <armadillo>
using namespace std;

// Gram-Schmidt process for orthonormalizing a set of vectors
int GramSchmidt(arma::mat& X) {

  // check that there is work to do
  if (X.n_cols < 1)  return 0;

  // get entry magnitude (for linear dependence check)
  double Xmax = arma::norm(X,"inf");

  // normalize first column
  double colnorm = arma::norm(X.col(0));
  if (colnorm < 1.e-13*Xmax) {
    cerr << "GramSchmidt error: vectors are linearly-dependent!\n";
    return 1;
  }
  X.col(0) *= (1.0/colnorm);

  // iterate over remaining vectors, performing Gram-Schmidt process
  for (int i=1; i<X.n_cols; i++) {

    // subtract off portions in directions of existing basis vectors
    for (int j=0; j<i; j++)
      X.col(i) -= (arma::dot(X.col(i), X.col(j)) * X.col(j));

    // normalize vector, checking for linear dependence
    colnorm = arma::norm(X.col(i));
    if (colnorm < 1.e-13*Xmax) {
      cerr << "GramSchmidt error: vectors are linearly-dependent!\n";
      return 1;
    }
    X.col(i) *= (1.0/colnorm);
  }

  // return success
  return 0;
}


#include "fastkpm.h"

#include <cassert>
#include <algorithm>
#include <boost/math/tools/roots.hpp>

#ifdef WITH_FFTW
#include <fftw3.h>
#endif

namespace fkpm {

    Vec<double> jackson_kernel(int M) {
        auto ret = Vec<double>(M);
        double Mp = M+1.0;
        for (int m = 0; m < M; m++) {
            ret[m] = (1.0/Mp)*((Mp-m)*cos(Pi*m/Mp) + sin(Pi*m/Mp)/tan(Pi/Mp));
        }
        return ret;
    }

    Vec<double> lorentz_kernel(int M, double lambda) {
        auto ret = Vec<double>(M);
        for (int m = 0; m < M; m++) {
            ret[m] = sinh(lambda * (1.0 - ((double) m)/ M)) / sinh(lambda);
        }
        return ret;
    }

    void chebyshev_fill_array(double x, Vec<double>& ret, int kind) {
        assert(kind == 1 || kind == 2);
        if (ret.size() > 0)
            ret[0] = 1.0;
        if (ret.size() > 1)
            ret[1] = kind * x;
        for (int m = 2; m < ret.size(); m++) {
            ret[m] = 2*x*ret[m-1] - ret[m-2];
        }
    }

    Vec<double> expansion_coefficients(int M, int Mq, std::function<double(double)> f, EnergyScale es) {
        assert(Mq >= M);
        auto kernel = jackson_kernel(M);
        auto ret = Vec<double>(M, 0.0);
#ifdef WITH_FFTW
        double *xc, *yc;
        xc = (double*) fftw_malloc(sizeof(double) * Mq);
        yc = (double*) fftw_malloc(sizeof(double) * Mq);
        for (int i = 0; i < Mq; i++) {
            double x_i = cos(Pi * (i+0.5) / Mq);
            xc[i] = f(es.unscale(x_i));
        }
        fftw_plan p;
        p = fftw_plan_r2r_1d(Mq, xc, yc, FFTW_REDFT10, FFTW_ESTIMATE);  // DCT-II
        fftw_execute(p);
        fftw_destroy_plan(p);
        for (int m = 0; m < M; m++) {
            ret[m] = (m == 0 ? 0.5 : 1.0) * kernel[m] * yc[m] / Mq;
        }
        fftw_free(xc);
        fftw_free(yc);
#else
        auto fp = Vec<double>(M, 0.0);
        auto T = Vec<double>(M);
        for (int i = 0; i < Mq; i++) {
            double x_i = cos(Pi * (i+0.5) / Mq);
            double f_i = f(es.unscale(x_i));
            chebyshev_fill_array(x_i, T);
            for (int m = 0; m < M; m++) {
                fp[m] += f_i * T[m];
            }
        }
        for (int m = 0; m < M; m++) {
            ret[m] = (m == 0 ? 1.0 : 2.0) * kernel[m] * fp[m] / Mq;
        }
#endif
        return ret;
    }

    Vec<Vec<cx_double>> electrical_conductivity_coefficients(int M, int Mq, double kT, double mu,
                                                             double omega, EnergyScale es, Vec<double> const& kernel) {
        assert(kernel.size() == M);
        assert(Mq >= 2*M);                                                      // To simplify usage of Y_{m_1+m_2} (see notes on fft)
        assert(omega >= 0.0);
        double cutoff = 1e-4;                                                   // neglect points near the boundary
        double omega_scaled = omega / es.mag();                                 // rescale omega
        Vec<Vec<cx_double>> ret(M);
        for (int i = 0; i < M; i++) {                                           // initialize cmn to 0
            ret[i].resize(M, 0.0);
        }
        if (omega_scaled >= 2.0 ) return ret;

        if (omega_scaled < 1e-10) {                                             // static conductivity
#ifdef WITH_FFTW
            double *x1, *x2, *yc1, *yc2, *ys1, *ys2;
            fftw_plan p1, p2, p3, p4;
            x1  = (double*) fftw_malloc(sizeof(double) * Mq);
            x2  = (double*) fftw_malloc(sizeof(double) * Mq);
            yc1 = (double*) fftw_malloc(sizeof(double) * Mq);
            yc2 = (double*) fftw_malloc(sizeof(double) * Mq);
            ys1 = (double*) fftw_malloc(sizeof(double) * Mq);
            ys2 = (double*) fftw_malloc(sizeof(double) * Mq);
            for (int i = 0; i < Mq; i++) {
                double x_i = cos(Pi * (i+0.5) / Mq);
                if (1.0-x_i*x_i < cutoff) {                                     // neglect points near boundary
                    x1[i] = 0.0;
                    x2[i] = 0.0;
                } else {
                    double f_i = fermi_density(es.unscale(x_i), kT, mu);
                    x1[i] = f_i * x_i / std::pow(1.0-x_i*x_i, 1.5);
                    x2[i] = f_i / (1.0-x_i*x_i);
                }
            }
            p1  = fftw_plan_r2r_1d(Mq, x1, yc1, FFTW_REDFT10, FFTW_ESTIMATE);   // DCT-II
            fftw_execute(p1);
            p2  = fftw_plan_r2r_1d(Mq, x2, yc2, FFTW_REDFT10, FFTW_ESTIMATE);   // DCT-II
            fftw_execute(p2);
            p3  = fftw_plan_r2r_1d(Mq, x1, ys1, FFTW_RODFT10, FFTW_ESTIMATE);   // DST-II
            fftw_execute(p3);
            p4  = fftw_plan_r2r_1d(Mq, x2, ys2, FFTW_RODFT10, FFTW_ESTIMATE);   // DST-II
            fftw_execute(p4);
            fftw_destroy_plan(p1);
            fftw_destroy_plan(p2);
            fftw_destroy_plan(p3);
            fftw_destroy_plan(p4);
            fftw_free(x1);
            fftw_free(x2);
            for (int m1 = 0; m1 < M; m1++) {
                double temp_m1 = Pi * (m1 == 0 ? 1.0 : 2.0) * kernel[m1] / (2.0 * Mq * es.mag() * es.mag());
                for (int m2 = 0; m2 < M; m2++) {
                    double temp_m2 = temp_m1 * (m2 == 0 ? 1.0 : 2.0) * kernel[m2];
                    int m_sum   = m1 + m2;
                    int m_dif   = m1 - m2;
                    double y_re =  2.0 * yc1[m_sum]
                                 + 2.0 * (m_dif >= 0 ? yc1[m_dif] : yc1[-m_dif])
                                 + m_sum * (m_sum > 0 ? ys2[m_sum-1] : 0.0)
                                 + m_dif * (m_dif > 0 ? ys2[m_dif-1] : (m_dif < 0 ? -ys2[-m_dif-1] : 0.0));
                    double y_im =  2.0 * (m_dif > 0 ? ys1[m_dif-1] : (m_dif < 0 ? -ys1[-m_dif-1] : 0.0))
                                 - m_dif * (yc2[m_sum] + (m_dif >=0 ? yc2[m_dif] : yc2[-m_dif]));
                    ret[m1][m2] = cx_double(temp_m2 * y_re, temp_m2 * y_im);
                }
            }
            fftw_free(yc1);
            fftw_free(yc2);
            fftw_free(ys1);
            fftw_free(ys2);
#else
            std::cout << "Warning: Not using FFTW (electrical_conductivity_coefficients)." << std::endl;
            auto T_i = Vec<double>(M);
            auto T_j = Vec<double>(M);
            for (int i = 0; i < Mq; i++) {
                double x_i = cos(Pi * (i+0.5) / Mq);
                if (1.0-x_i*x_i < cutoff) continue;                             // neglect points near boundary
                double temp_squareroot1 = std::sqrt(1.0-x_i*x_i);
                double f_i;
                chebyshev_fill_array(x_i, T_i);
                chebyshev_fill_array(x_i, T_j, 2);                              // fill T_j with sin[m * arccos(x)]
                for (int m = M-1; m > 0; m--) {
                    T_j[m] = T_j[m-1] * temp_squareroot1;
                }
                T_j[0] = 0.0;
                f_i = fermi_density(es.unscale(x_i), kT, mu) / std::pow(1.0-x_i*x_i, 1.5);
                for (int m1 = 0; m1 < M; m1++) {
                    for (int m2 = 0; m2 < M; m2++) {
                        ret[m1][m2] += ( T_i[m1] * cx_double(T_i[m2],-T_j[m2])
                                        * cx_double(x_i, m2 * temp_squareroot1)
                                        +T_i[m2] * cx_double(T_i[m1], T_j[m1])
                                        * cx_double(x_i,-m1 * temp_squareroot1) ) * f_i;
                    }
                }
            }
            for (int m1 = 0; m1 < M; m1++) {
                double temp_m1 = 2.0 * Pi * (m1 == 0 ? 1.0 : 2.0) * kernel[m1] / (Mq * es.mag() * es.mag());
                for (int m2 = 0; m2 < M; m2++) {
                    double temp_m2 = temp_m1 * (m2 == 0 ? 1.0 : 2.0) * kernel[m2];
                    ret[m1][m2] *= cx_double(temp_m2, 0.0);
                }
            }
#endif
        } else {                                                                // optical conductivity
            std::cout << "Warning: FFTW not implemented yet (electrical_conductivity_coefficients)." << std::endl;
            int i_start = std::ceil(acos(1.0 - omega_scaled) / Pi * Mq - 0.5);
            assert(M - i_start >= 20);                                          // at least 20 points to do integration
            auto T_i = Vec<double>(M);
            auto T_j = Vec<double>(M);
            for (int i = i_start; i < Mq; i++) {
                double x_i = cos(Pi * (i+0.5) / Mq);
                if (1.0-x_i*x_i < cutoff) continue;                             // neglect points near boundary
                double temp_squareroot2 = std::sqrt(1.0 - (x_i + omega_scaled) * (x_i + omega_scaled));
                double f_i;
                chebyshev_fill_array(x_i + omega_scaled, T_i);
                chebyshev_fill_array(x_i, T_j);           // fill T_j with cos[m * arccos(x)]
                f_i = (fermi_density(es.unscale(x_i), kT, mu) - fermi_density(es.unscale(x_i) + omega, kT, mu))
                     / (omega * temp_squareroot2);
                for (int m1 = 0; m1 < M; m1++) {
                    for (int m2 = 0; m2 < M; m2++) {
                        ret[m1][m2] += T_i[m1] * T_j[m2] * f_i;
                    }
                }
            }
            for (int m1 = 0; m1 < M; m1++) {
                double temp_m1 = 2.0 * Pi * (m1 == 0 ? 1.0 : 2.0) * kernel[m1] / (Mq * es.mag() * es.mag());
                for (int m2 = 0; m2 < M; m2++) {
                    double temp_m2 = temp_m1 * (m2 == 0 ? 1.0 : 2.0) * kernel[m2];
                    ret[m1][m2] *= cx_double(temp_m2, 0.0);
                }
            }

        }
        return ret;
    }

    Vec<double> moment_transform(Vec<double> const& moments, int Mq) {
        int M = moments.size();
        auto T = Vec<double>(M);
        auto mup = Vec<double>(M);
        auto gamma = Vec<double>(Mq);

        auto kernel = jackson_kernel(M);
        for (int m = 0; m < M; m++)
            mup[m] = moments[m] * kernel[m];

        // TODO: replace with DCT-III, mup -> gamma (caution, double check FFTW docs)
        for (int i = 0; i < Mq; i++) {
            gamma[i] = 0.0;
            double x_i = cos(Pi * (i+0.5) / Mq);
            chebyshev_fill_array(x_i, T); // T_m(x_i) = cos(m pi (i+1/2) / Mq)
            for (int m = 0; m < M; m++) {
                gamma[i] += (m == 0 ? 1 : 2) * mup[m] * T[m];
            }
        }
        return gamma;
    }

    Vec<Vec<cx_double>> moment_transform(Vec<Vec<cx_double>> const& moments, int Mq, Vec<double> const& kernel) {
        int M = moments.size();
        auto T_i = Vec<double>(M);
        auto T_j = Vec<double>(M);
        Vec<Vec<cx_double>> mup(M);
        Vec<Vec<cx_double>> gamma(Mq);
        for (int i = 0; i < M; i++)  mup[i].resize(M, cx_double(0.0,0.0));
        for (int i = 0; i < Mq; i++) gamma[i].resize(Mq, cx_double(0.0,0.0));

        for (int m1 = 0; m1 < M; m1++) {
            for (int m2 = 0; m2 < M; m2++) {
                mup[m1][m2] = cx_double((m1 == 0 ? 1.0 : 2.0) * (m2 == 0 ? 1.0 : 2.0)
                                        * kernel[m1] * kernel[m2], 0.0) * moments[m1][m2];
            }
        }

        // TODO replace with fftw
        for (int i = 0; i < Mq; i++) {
            double x_i = cos(Pi * (i+0.5) / Mq);
            chebyshev_fill_array(x_i, T_i);
            for (int j = 0; j < Mq; j++) {
                double x_j = cos(Pi * (j+0.5) / Mq);
                chebyshev_fill_array(x_j, T_j);
                for (int m1 = 0; m1 < M; m1++) {
                    for (int m2 = 0; m2 < M; m2++) {
                        gamma[i][j] += (T_i[m1] * T_j[m2]) * mup[m1][m2];
                    }
                }
            }
        }
        return gamma;
    }

    double moment_product(Vec<double> const& c, Vec<double> const& mu) {
        int M = c.size();
        double ret = 0;
        for (int i = 0; i < M; i++) {
            ret += c[i]*mu[i];
        }
        return ret;
    }

    cx_double moment_product(Vec<Vec<cx_double>> const& c, Vec<Vec<cx_double>> const& mu) {
        int M1 = c.size();
        int M2 = c[0].size();
        assert(mu.size() == M1);
        assert(mu[0].size() == M2);
        cx_double ret(0.0, 0.0);
        for (int m1 = 0; m1 < M1; m1++) {
            for (int m2 = 0; m2 < M2; m2++) {
                ret += c[m1][m2] * mu[m1][m2];
            }
        }
        return ret;
    }

    double density_product(Vec<double> const& gamma, std::function<double(double)> f, EnergyScale es) {
        int Mq = gamma.size();
        double ret = 0.0;
        for (int i = 0; i < Mq; i++) {
            double x_i = cos(Pi * (i+0.5) / Mq);
            ret += gamma[i] * f(es.unscale(x_i));
        }
        return ret / Mq;
    }

    void density_function(Vec<double> const& gamma, EnergyScale es, Vec<double>& x, Vec<double>& rho) {
        int Mq = gamma.size();
        x.resize(Mq);
        rho.resize(Mq);
        for (int i = Mq-1; i >= 0; i--) {
            double x_i = cos(Pi * (i+0.5) / Mq);
            x[Mq-1-i] = es.unscale(x_i);
            rho[Mq-1-i] = gamma[i] / (Pi * sqrt(1-x_i*x_i) * es.mag());
        }
    }

    void density_function(Vec<Vec<cx_double>> const& gamma, EnergyScale es, Vec<double>& x, Vec<double>& y, Vec<Vec<cx_double>>& rho) {
        int Mq1 = gamma.size();
        int Mq2 = gamma[0].size();
        x.resize(Mq1);
        y.resize(Mq2);
        rho.resize(Mq1);
        for (int i = 0; i < Mq1; i++) {
            rho[i].resize(Mq2);
            for (int j = 0; j < Mq2; j++) {
                rho[i][j] = 0.0;
            }
        }
        for (int i2 = Mq2-1; i2 >= 0; i2--) {
            double x2   = cos(Pi * (i2+0.5) / Mq2);
            y[Mq2-1-i2] = es.unscale(x2);
        }
        for (int i1 = Mq1-1; i1 >= 0; i1--) {
            double x1   = cos(Pi * (i1+0.5) / Mq1);
            x[Mq1-1-i1] = es.unscale(x1);
            for (int i2 = Mq2-1; i2 >= 0; i2--) {
                double x2 = cos(Pi * (i2+0.5) / Mq2);
                rho[Mq1-1-i1][Mq2-1-i2] = gamma[i1][i2] / (Pi * sqrt(1.0 - x1*x1)
                                                          * Pi * sqrt(1.0 - x2*x2) * es.mag() * es.mag());
            }
        }
    }

    void integrated_density_function(Vec<double> const& gamma, EnergyScale es, Vec<double>& x, Vec<double>& irho) {
        int Mq = gamma.size();
        x.resize(Mq);
        irho.resize(Mq);
        double acc = 0.0;
        for (int i = Mq-1; i >= 0; i--) {
            double x_i = cos(Pi * (i+0.5) / Mq);
            x[Mq-1-i] = es.unscale(x_i);
            irho[Mq-1-i] = (acc+0.5*gamma[i]) / Mq;
            acc += gamma[i];
        }
    }

    double fermi_energy(double x, double kT, double mu) {
        double alpha = (x-mu)/std::abs(kT);
        if (kT < 1e-15 || std::abs(alpha) > 20) {
            return (x < mu) ? (x-mu) : 0.0;
        }
        else {
            return -kT*log(1 + exp(-alpha));
        }
    }

    double fermi_density(double x, double kT, double mu) {
        double alpha = (x-mu)/std::abs(kT);
        if (kT < 1e-15 || std::abs(alpha) > 20) {
            return (x < mu) ? 1.0 : 0.0;
        }
        else {
            return 1.0/(exp(alpha)+1.0);
        }
    }

    double mu_to_filling(Vec<double> const& gamma, EnergyScale const& es, double kT, double mu) {
        using std::placeholders::_1;
        double n_occ = density_product(gamma, std::bind(fermi_density, _1, kT, mu), es);
        double n_tot = density_product(gamma, [](double x){return 1;}, es);
        return n_occ/n_tot;
    }
    double mu_to_filling(arma::vec const& evals, double kT, double mu) {
        double n_occ = 0;
        double n_tot = evals.size();
        for (double const& x : evals) {
            n_occ += fermi_density(x, kT, mu);
        }
        return n_occ/n_tot;
    }

    static double root_solver(std::function<double(double)> f, double lo, double hi) {
        int precision_bits = 30;
        boost::math::tools::eps_tolerance<double> tol(precision_bits);
        boost::uintmax_t max_iter=50;
        auto bds = boost::math::tools::toms748_solve(f, lo, hi, tol, max_iter);
        return 0.5 * (bds.first + bds.second);
    }

    double filling_to_mu(Vec<double> const& gamma, EnergyScale const& es, double kT, double filling, double delta_filling) {
        // thermal smearing for faster convergence
        kT = std::max(kT, 0.1*es.mag()/gamma.size());
        double c = kT * std::log(1.0/filling - 1.0);

        auto f1 = [&](double x) { return mu_to_filling(gamma, es, kT, x) - (filling+delta_filling); };
        auto f2 = [&](double x) { return mu_to_filling(gamma, es, kT, x) - (filling-delta_filling); };
        if (delta_filling == 0) {
            return root_solver(f1, es.lo - std::max(c,0.0), es.hi + std::max(-c,0.0));
        }
        else {
            return 0.5 * (root_solver(f1, es.lo - std::max(c,0.0), es.hi + std::max(-c,0.0))
                        + root_solver(f2, es.lo - std::max(c,0.0), es.hi + std::max(-c,0.0)));
        }
    }
    double filling_to_mu(arma::vec const& evals, double kT, double filling) {
        assert(kT > 0 && "filling_to_mu() requires thermal smearing!");
        auto f = [&](double x) { return mu_to_filling(evals, kT, x) - filling; };
        auto minmax = std::minmax_element(evals.begin(), evals.end());
        return root_solver(f, *minmax.first, *minmax.second);
    }

    double electronic_grand_energy(Vec<double> const& gamma, EnergyScale const& es, double kT, double mu) {
        using std::placeholders::_1;
        return density_product(gamma, std::bind(fermi_energy, _1, kT, mu), es);
    }
    double electronic_grand_energy(arma::vec const& evals, double kT, double mu) {
        double acc = 0;
        for (double const& x : evals) {
            acc += fermi_energy(x, kT, mu);
        }
        return acc;
    }

    double electronic_energy(Vec<double> const& gamma, EnergyScale const& es, double kT, double filling, double mu) {
        double n_tot = density_product(gamma, [](double x){return 1;}, es);
        double n_occ = filling*n_tot;
        return electronic_grand_energy(gamma, es, kT, mu) + mu*n_occ;
    }
    double electronic_energy(arma::vec const& evals, double kT, double filling) {
        // at zero temperature, need special logic to correctly count degenerate eigenvalues
        if (kT == 0) {
            auto evals_sorted = evals;
            std::sort(evals_sorted.begin(), evals_sorted.end());
            int n_occ = int(filling*evals.size() + 0.5);
            assert(n_occ >= 0 && n_occ <= evals.size());
            double acc = 0;
            for (int i = 0; i < n_occ; i++) {
                acc += evals_sorted[i];
            }
            return acc;
        }
        else {
            double n_tot = evals.size();
            double n_occ = filling*n_tot;
            double mu = filling_to_mu(evals, kT, filling);
            return electronic_grand_energy(evals, kT, mu) + mu*n_occ;
        }
    }

}