// -*- coding: utf-16 -*-
#pragma once

/** @file include/fractions.hpp
 *  This is a C++ Library header.
 */

// #include <boost/operators.hpp>
// #include <cmath>
#include <numeric>
#include <type_traits>
#include <utility>

#include "common_concepts.h"

namespace fun {

    /**
     * @brief absolute
     *
     * @tparam T
     * @param[in] a
     * @return T
     */
    template <typename T> inline constexpr auto abs(const T& a) -> T {
        if constexpr (std::is_unsigned_v<T>) {
            return a;
        } else {
            return (a < T(0)) ? -a : a;
        }
    }

    /**
     * @brief Greatest common divider
     *
     * @tparam _Mn
     * @param[in] __m
     * @param[in] __n
     * @return _Mn
     */
    template <Integral _Mn> inline constexpr auto gcd_recur(const _Mn& __m, const _Mn& __n) -> _Mn {
        if (__n == 0) {
            return abs(__m);
        }
        return gcd_recur(__n, __m % __n);
    }

    /**
     * @brief Greatest common divider
     *
     * @tparam _Mn
     * @param[in] __m
     * @param[in] __n
     * @return _Mn
     */
    template <Integral _Mn> inline constexpr auto gcd(const _Mn& __m, const _Mn& __n) -> _Mn {
        if (__m == 0) {
            return abs(__n);
        }
        return gcd_recur(__m, __n);
    }

    /**
     * @brief Least common multiple
     *
     * @tparam _Mn
     * @param[in] __m
     * @param[in] __n
     * @return _Mn
     */
    template <Integral _Mn> inline constexpr auto lcm(const _Mn& __m, const _Mn& __n) -> _Mn {
        if (__m == 0 || __n == 0) {
            return 0;
        }
        return (abs(__m) / gcd(__m, __n)) * abs(__n);
    }

    /**
     * @brief Fraction
     *
     * @tparam Z
     */
    template <Integral Z> struct Fraction {
        Z _num;
        Z _den;

        /**
         * @brief Construct a new Fraction object
         *
         * @param[in] num
         * @param[in] den
         */
        constexpr Fraction(Z num, Z den) : _num{std::move(num)}, _den{std::move(den)} {
            this->normalize();
        }

        /**
         * @brief normalize to a canonical form
         *
         * denominator is always non-negative and co-prime with numerator
         */
        constexpr auto normalize() -> Z {
            this->normalize1();
            return this->normalize2();
        }

        /**
         * @brief normalize to a canonical form
         *
         * denominator is always non-negative
         */
        constexpr void normalize1() {
            if (this->_den < Z(0)) {
                this->_num = -this->_num;
                this->_den = -this->_den;
            }
        }

        /**
         * @brief normalize to a canonical form
         *
         * denominator is always co-prime with numerator
         */
        constexpr auto normalize2() -> Z {
            Z common = gcd(this->_num, this->_den);
            if (common == Z(1) || common == Z(0)) {
                return common;
            }
            this->_num /= common;
            this->_den /= common;
            return common;
        }

        /**
         * @brief Construct a new Fraction object
         *
         * @param[in] num
         */
        constexpr explicit Fraction(Z&& num) : _num{std::move(num)}, _den(Z(1)) {}

        /**
         * @brief Construct a new Fraction object
         *
         * @param[in] num
         */
        constexpr explicit Fraction(const Z& num) : _num{num}, _den(1) {}

        /**
         * @brief Construct a new Fraction object
         *
         * @param[in] num
         */
        constexpr Fraction() : _num(0), _den(1) {}

        /**
         * @brief
         *
         * @return const Z&
         */
        [[nodiscard]] constexpr auto num() const noexcept -> const Z& { return _num; }

        /**
         * @brief
         *
         * @return const Z&
         */
        [[nodiscard]] constexpr auto den() const noexcept -> const Z& { return _den; }

        /**
         * @brief cross product
         *
         * @param rhs
         * @return Z
         */
        constexpr auto cross(const Fraction& rhs) const -> Z {
            return this->_num * rhs._den - this->_den * rhs._num;
        }

        /** @name Comparison operators
         *  ==, !=, <, >, <=, >= etc.
         */
        ///@{

        /**
         * @brief Equal to
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator==(Fraction lhs, Z rhs) -> bool {
            if (lhs._den == Z(1) || rhs == Z(0)) {
                return lhs._num == rhs;
            }
            std::swap(lhs._den, rhs);
            lhs.normalize2();
            return lhs._num == lhs._den * rhs;
        }

        /**
         * @brief Less than
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator<(Fraction lhs, Z rhs) -> bool {
            if (lhs._den == Z(1) || rhs == Z(0)) {
                return lhs._num == rhs;
            }
            std::swap(lhs._den, rhs._num);
            lhs.normalize2();
            return lhs._num < lhs._den * rhs;
        }

        /**
         * @brief Less than
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator<(Z lhs, Fraction rhs) -> bool {
            if (rhs._den == Z(1) || lhs == Z(0)) {
                return lhs < rhs._num;
            }
            std::swap(rhs._den, lhs);
            rhs.normalize2();
            return rhs._den * lhs < rhs._num;
        }

        /**
         * @brief Equal to
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator==(const Z& lhs, const Fraction& rhs) -> bool {
            return rhs == lhs;
        }

        /**
         * @brief Equal to
         *
         * @param[in] rhs
         * @return true
         * @return false
         */

        /**
         * @brief Equal to
         *
         * @param lhs
         * @param rhs
         * @return true
         * @return false
         */
        constexpr friend auto operator==(Fraction lhs, Fraction rhs) -> bool {
            if (lhs._den == rhs._den) {
                return lhs._num == rhs._num;
            }
            std::swap(lhs._den, rhs._num);
            lhs.normalize2();
            rhs.normalize2();
            return lhs._num * rhs._den == lhs._den * rhs._num;
        }

        /**
         * @brief Less than
         *
         * @param lhs
         * @param rhs
         * @return true
         * @return false
         */
        constexpr friend auto operator<(Fraction lhs, Fraction rhs) -> bool {
            if (lhs._den == rhs._den) {
                return lhs._num < rhs._num;
            }
            std::swap(lhs._den, rhs._num);
            lhs.normalize2();
            rhs.normalize2();
            return lhs._num * rhs._den < lhs._den * rhs._num;
        }

        /**
         * @brief
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator!=(const Fraction& rhs) const -> bool { return !(*this == rhs); }

        /**
         * @brief Greater than
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator>(const Fraction& rhs) const -> bool { return rhs < *this; }

        /**
         * @brief Greater than or euqal to
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator>=(const Fraction& rhs) const -> bool { return !(*this < rhs); }

        /**
         * @brief Less than or equal to
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator<=(const Fraction& rhs) const -> bool { return !(rhs < *this); }

        /**
         * @brief Greater than
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator>(const Z& rhs) const -> bool { return rhs < *this; }

        /**
         * @brief Less than or equal to
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator<=(const Z& rhs) const -> bool { return !(rhs < *this); }

        /**
         * @brief Greater than or equal to
         *
         * @param[in] rhs
         * @return true
         * @return false
         */
        constexpr auto operator>=(const Z& rhs) const -> bool { return !(*this < rhs); }

        /**
         * @brief Greater than
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator>(const Z& lhs, const Fraction& rhs) -> bool {
            return rhs < lhs;
        }

        /**
         * @brief Less than or equal to
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator<=(const Z& lhs, const Fraction& rhs) -> bool {
            return !(rhs < lhs);
        }

        /**
         * @brief Greater than or euqal to
         *
         * @param[in] lhs
         * @param[in] rhs
         * @return true
         * @return false
         */
        friend constexpr auto operator>=(const Z& lhs, const Fraction& rhs) -> bool {
            return !(lhs < rhs);
        }

        ///@}

        /**
         * @brief reciprocal
         *
         */
        constexpr void reciprocal() noexcept(std::is_nothrow_swappable_v<Z>) {
            std::swap(this->_num, this->_den);
            this->normalize1();
        }

        /**
         * @brief multiply and assign
         *
         * @param rhs
         * @return Fraction&
         */
        constexpr auto operator*=(Fraction rhs) -> Fraction& {
            std::swap(this->_num, rhs._num);
            this->normalize2();
            rhs.normalize2();
            this->_num *= rhs._num;
            this->_den *= rhs._den;
            return *this;
        }

        /**
         * @brief multiply
         *
         * @param lhs
         * @param rhs
         * @return Fraction
         */
        friend constexpr auto operator*(Fraction lhs, const Fraction& rhs) -> Fraction {
            return lhs *= rhs;
        }

        /**
         * @brief multiply and assign
         *
         * @param rhs
         * @return Fraction&
         */
        constexpr auto operator*=(Z rhs) -> Fraction& {
            std::swap(this->_num, rhs);
            this->normalize2();
            this->_num *= rhs;
            return *this;
        }

        /**
         * @brief multiply
         *
         * @param lhs
         * @param rhs
         * @return Fraction
         */
        friend constexpr auto operator*(Fraction lhs, const Z& rhs) -> Fraction {
            return lhs *= rhs;
        }

        /**
         * @brief multiply
         *
         * @param lhs
         * @param rhs
         * @return Fraction
         */
        friend constexpr auto operator*(const Z& lhs, Fraction rhs) -> Fraction {
            return rhs *= lhs;
        }

        /**
         * @brief divide and assign
         *
         * @param rhs
         * @return Fraction&
         */
        constexpr auto operator/=(Fraction rhs) -> Fraction& {
            std::swap(this->_den, rhs._num);
            this->normalize();
            rhs.normalize2();
            this->_num *= rhs._den;
            this->_den *= rhs._num;
            return *this;
        }

        /**
         * @brief divide
         *
         * @param lhs
         * @param rhs
         * @return Fraction
         */
        friend constexpr auto operator/(Fraction lhs, const Fraction& rhs) -> Fraction {
            return lhs /= rhs;
        }

        /**
         * @brief divide and assign
         *
         * @param rhs
         * @return Fraction&
         */
        constexpr auto operator/=(const Z& rhs) -> Fraction& {
            std::swap(this->_den, rhs);
            this->normalize();
            this->_den *= rhs;
            return *this;
        }

        /**
         * @brief divide
         *
         * @param lhs
         * @param rhs
         * @return Fraction
         */
        friend constexpr auto operator/(Fraction lhs, const Z& rhs) -> Fraction {
            return lhs /= rhs;
        }

        /**
         * @brief divide
         *
         * @param lhs
         * @param rhs
         * @return Fraction
         */
        friend constexpr auto operator/(const Z& lhs, Fraction rhs) -> Fraction {
            rhs.reciprocal();
            return rhs *= lhs;
        }

        /**
         * @brief Negate
         *
         * @return Fraction
         */
        constexpr auto operator-() const -> Fraction {
            auto res = Fraction(*this);
            res._num = -res._num;
            return res;
        }

        /**
         * @brief Add
         *
         * @param rhs
         * @return Fraction
         */
        constexpr auto operator+(const Fraction& rhs) const -> Fraction {
            if (this->_den == rhs._den) {
                return Fraction(this->_num + rhs._num, this->_den);
            }
            const auto common = gcd(this->_den, rhs._den);
            if (common == Z(0)) {
                return Fraction(rhs._den * this->_num + this->_den * rhs._num, Z(0));
            }
            const auto l = this->_den / common;
            const auto r = rhs._den / common;
            auto d = this->_den * r;
            auto n = r * this->_num + l * rhs._num;
            return Fraction(std::move(n), std::move(d));
        }

        /**
         * @brief Subtract
         *
         * @param[in] frac
         * @return Fraction
         */
        constexpr auto operator-(const Fraction& frac) const -> Fraction { return *this + (-frac); }

        /**
         * @brief Add
         *
         * @param[in] frac
         * @param[in] i
         * @return Fraction
         */
        friend constexpr auto operator+(Fraction frac, const Z& i) -> Fraction { return frac += i; }

        /**
         * @brief Add
         *
         * @param[in] i
         * @param[in] frac
         * @return Fraction
         */
        friend constexpr auto operator+(const Z& i, Fraction frac) -> Fraction { return frac += i; }

        /**
         * @brief
         *
         * @param[in] i
         * @return Fraction
         */
        constexpr auto operator-(const Z& i) const -> Fraction { return *this + (-i); }

        /**
         * @brief
         *
         * @param[in] rhs
         * @return Fraction
         */
        constexpr auto operator+=(const Fraction& rhs) -> Fraction& { return *this -= (-rhs); }

        /**
         * @brief
         *
         * @param[in] rhs
         * @return Fraction
         */
        constexpr auto operator-=(const Fraction& rhs) -> Fraction& {
            if (this->_den == rhs._den) {
                this->_num -= rhs._num;
                this->normalize2();
                return *this;
            }

            auto other{rhs};
            std::swap(this->_den, other._num);
            auto common_n = this->normalize2();
            auto common_d = other.normalize2();
            std::swap(this->_den, other._num);
            this->_num = this->cross(other);
            this->_den *= other._den;
            std::swap(this->_den, common_d);
            this->normalize2();
            this->_num *= common_n;
            this->_den *= common_d;
            this->normalize2();
            return *this;
        }

        /**
         * @brief
         *
         * @param[in] i
         * @return Fraction
         */
        constexpr auto operator+=(const Z& i) -> Fraction& { return *this -= (-i); }

        /**
         * @brief
         *
         * @param[in] rhs
         * @return Fraction
         */
        constexpr auto operator-=(const Z& rhs) -> Fraction& {
            if (this->_den == Z(1)) {
                this->_num -= rhs;
                return *this;
            }

            auto other{rhs};
            std::swap(this->_den, other);
            auto common_n = this->normalize2();
            std::swap(this->_den, other);
            this->_num -= other * this->_den;
            this->_num *= common_n;
            this->normalize2();
            return *this;
        }

        /**
         * @brief
         *
         * @param[in] c
         * @param[in] frac
         * @return Fraction
         */
        friend constexpr auto operator-(const Z& c, const Fraction& frac) -> Fraction {
            return c + (-frac);
        }

        /**
         * @brief
         *
         * @param[in] c
         * @param[in] frac
         * @return Fraction
         */
        friend constexpr auto operator+(int&& c, const Fraction& frac) -> Fraction {
            return frac + Z(c);
        }

        /**
         * @brief
         *
         * @param[in] c
         * @param[in] frac
         * @return Fraction
         */
        friend constexpr auto operator-(int&& c, const Fraction& frac) -> Fraction {
            return (-frac) + Z(c);
        }

        /**
         * @brief
         *
         * @param[in] c
         * @param[in] frac
         * @return Fraction<Z>
         */
        friend constexpr auto operator*(int&& c, const Fraction& frac) -> Fraction {
            return frac * Z(c);
        }

        /**
         * @brief
         *
         * @tparam _Stream
         * @tparam Z
         * @param[in] os
         * @param[in] frac
         * @return _Stream&
         */
        template <typename _Stream> friend auto operator<<(_Stream& os, const Fraction& frac)
            -> _Stream& {
            os << "(" << frac.num() << "/" << frac.den() << ")";
            return os;
        }
    };

    // For template deduction
    // Integral{Z} Fraction(const Z &, const Z &) noexcept -> Fraction<Z>;

}  // namespace fun


// $ g++ -std=c++14 -I/usr/include/eigen3 coordinateTransform.cpp
//
#include <iostream>
#include <vector>
#include <algorithm>
#include <iomanip>

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

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize();
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;

  // ---
  auto w1 = T1w.inverse() * p1;
  cout << "word1 cor=" << w1 << endl;

  auto w2 = T2w.inverse() * p2;
  cout << "word2 cor=" << w2 << endl;
  // https://stackoverflow.com/questions/15051367/how-to-compare-vectors-approximately-in-eigen
  // There is also isApprox function which was not working for me. I am just using ( expect - res).norm() < some small number.
  cout << w1.x() << ","<< w2.x() << " and they are equal?"<< (w1.x() == w2.x()) <<endl;
  auto diff = (w2 - w1).norm();
  cout << std::setprecision(10) << std::fixed<< diff << endl;
  cout << "std::setprecision(10): " << std::setprecision(10) << std::fixed<< diff << '\n'
       << "max precision:        " << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << diff << endl;
  return 0;
}


// tests/unit/stanmath/StanPerceptronTest_unittest.cc
#include <Eigen/Dense>
#include <cmath>
#include <gtest/gtest.h>
#include <iostream>
#include <random>
#include <stan/math.hpp>
using namespace std;
using namespace Eigen;
using namespace stan::math;

/*
This test will implement a perceptron using stan math
The perceptron will have one input, the column vector [1 1 1]^T
Then, it will have 2x3 Matrix of weights that it will learn
Then, the output is a 2x1 column vector
In this example, we want to learn the weights W such that the square
Error loss from the output of the perceptron to [1 1]^T is minimized.
Since we can find weights from [1 1 1]^T to [1 1]^T in a perceptron,
this error should be very close to zero after 100 epochs.
*/
TEST(StanPerceptronTest, sample_perceptron)
{
	// Initialize the Input Vector
	Matrix<var, 3, 1> inp;
	inp(0, 0) = 1;
	inp(1, 0) = 1;
	inp(2, 0) = 1;

	// Randomly Initialize the weights on the perceptron
	std::random_device rd{};
	std::mt19937 gen{rd()};
	normal_distribution<> d{0, 1};
	Matrix<var, 2, 3> W1;
	for (int i = 0; i < 2; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			W1(i, j) = 0.01 * d(gen);
		}
	}

	// Define the outputs of the neural network
	Matrix<var, 2, 1> outputs;

	double learning_rate = 0.1;
	double last_error = 0;
	for (int epoch = 0; epoch < 100; ++epoch)
	{
		var error = 0;
		outputs = W1 * inp;
		for (int i = 0; i < 2; ++i)
		{
			error += (outputs(i, 0) - 1) * (outputs(i, 0) - 1);
		}
		error.grad();

		// Now use gradient descent to change the weights
		for (int i = 0; i < 2; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				W1(i, j) = W1(i, j) - learning_rate * W1(i, j).adj();
			}
		}

		// Store the value of current error in last_error
		last_error = value_of(error);
	}

	// Error should be very close to 0.0
	EXPECT_NEAR(last_error, 0.0, 1e-6);
}


//
//  interp.hpp
//  test_proj
//
//  Created by Erlend Basso on 08/03/2021.
//

#ifndef interp_h
#define interp_h

#include <Eigen/Dense>

// M: M \times M matrix of data to interpolate from
template <unsigned int M>

class BilinearInterpolator
{
public:
    using VectorMd = Eigen::Matrix<double, M, 1>;
    using MatrixMd = Eigen::Matrix<double, M, M>;
    using Vector2d = Eigen::Vector2d;

    BilinearInterpolator() {}

    BilinearInterpolator(const MatrixMd &F, const VectorMd &breakpoints_x, const VectorMd &breakpoints_y)
        : F_{F},
          breakpoints_x_{breakpoints_x},
          breakpoints_y_{breakpoints_y}
    {
    }

    void init(const MatrixMd &F, const VectorMd &breakpoints_x, const VectorMd &breakpoints_y)
    {
        F_ = F;
        breakpoints_x_ = breakpoints_x;
        breakpoints_y_ = breakpoints_y;
    }

    double interp(double x, double y)
    {
        int ind_x = 0;
        int ind_y = 0;

        for (int i = 0; i < M - 1; i++)
        {
            if (x >= breakpoints_x_(i) && x <= breakpoints_x_(i + 1))
            {
                x1_ = breakpoints_x_(i);
                x2_ = breakpoints_x_(i + 1);
                ind_x = i;
                break;
            }
        }
        for (int j = 0; j < M - 1; j++)
        {
            if (y >= breakpoints_y_(j) && y <= breakpoints_y_(j + 1))
            {
                y1_ = breakpoints_y_(j);
                y2_ = breakpoints_y_(j + 1);
                ind_y = j;
                break;
            }
        }

        Vector2d delta_x{x2_ - x, x - x1_};
        Vector2d delta_y{y2_ - y, y - y1_};

        return 1.0 / ((x2_ - x1_) * (y2_ - y1_)) * delta_x.transpose() * F_.block(ind_x, ind_y, 2, 2) * delta_y;
    }

private:
    MatrixMd F_;
    VectorMd breakpoints_x_;
    VectorMd breakpoints_y_;
    double x1_, x2_, y1_, y2_;
};

#endif /* interp_h */


//  (C) Copyright Nick Thompson 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#ifndef BOOST_MATH_TOOLS_CUBIC_ROOTS_HPP
#define BOOST_MATH_TOOLS_CUBIC_ROOTS_HPP
#include <array>
#include <algorithm>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/tools/roots.hpp>

namespace boost::math::tools {

// Solves ax^3 + bx^2 + cx + d = 0.
// Only returns the real roots, as types get weird for real coefficients and complex roots.
// Follows Numerical Recipes, Chapter 5, section 6.
// NB: A better algorithm apparently exists:
// Algorithm 954: An Accurate and Efficient Cubic and Quartic Equation Solver for Physical Applications
// However, I don't have access to that paper!
template<typename Real>
std::array<Real, 3> cubic_roots(Real a, Real b, Real c, Real d) {
    using std::sqrt;
    using std::acos;
    using std::cos;
    using std::cbrt;
    using std::abs;
    using std::fma;
    std::array<Real, 3> roots = {std::numeric_limits<Real>::quiet_NaN(),
                                 std::numeric_limits<Real>::quiet_NaN(),
                                 std::numeric_limits<Real>::quiet_NaN()};
    if (a == 0) {
        // bx^2 + cx + d = 0:
        if (b == 0) {
            // cx + d = 0:
            if (c == 0) {
                if (d != 0) {
                    // No solutions:
                    return roots;
                }
                roots[0] = 0;
                roots[1] = 0;
                roots[2] = 0;
                return roots;
            }
            roots[0] = -d/c;
            return roots;
        }
        auto [x0, x1] = quadratic_roots(b, c, d);
        roots[0] = x0;
        roots[1] = x1;
        return roots;
    }
    if (d == 0) {
        auto [x0, x1] = quadratic_roots(a, b, c);
        roots[0] = x0;
        roots[1] = x1;
        roots[2] = 0;
        std::sort(roots.begin(), roots.end());
        return roots;
    }
    Real p = b/a;
    Real q = c/a;
    Real r = d/a;
    Real Q = (p*p - 3*q)/9;
    Real R = (2*p*p*p - 9*p*q + 27*r)/54;
    if (R*R < Q*Q*Q) {
        Real rtQ = sqrt(Q);
        Real theta = acos(R/(Q*rtQ))/3;
        Real st = sin(theta);
        Real ct = cos(theta);
        roots[0] = -2*rtQ*ct - p/3;
        roots[1] = -rtQ*(-ct + sqrt(Real(3))*st) - p/3;
        roots[2] = rtQ*(ct + sqrt(Real(3))*st) - p/3;
    } else {
        // In Numerical Recipes, Chapter 5, Section 6, it is claimed that we only have one real root
        // if R^2 >= Q^3. But this isn't true; we can even see this from equation 5.6.18.
        // The condition for having three real roots is that A = B.
        // It *is* the case that if we're in this branch, and we have 3 real roots, two are a double root.
        // Take (x+1)^2(x-2) = x^3 - 3x -2 as an example. This clearly has a double root at x = -1,
        // and it gets sent into this branch.
        Real arg = R*R - Q*Q*Q;
        Real A = -boost::math::sign(R)*cbrt(abs(R) + sqrt(arg));
        Real B = 0;
        if (A != 0) {
            B = Q/A;
        }
        roots[0] = A + B - p/3;
        // Yes, we're comparing floats for equality:
        // Any perturbation pushes the roots into the complex plane; out of the bailiwick of this routine.
        if (A == B || arg == 0) {
            roots[1] = -A - p/3;
            roots[2] = -A - p/3;
        }
    }
    // Root polishing:
    for (auto & r : roots) {
        // Horner's method.
        // Here I'll take John Gustaffson's opinion that the fma is a *distinct* operation from a*x +b:
        // Make sure to compile these fmas into a single instruction and not a function call!
        // (I'm looking at you Windows.)
        Real f = fma(a, r, b);
        f = fma(f,r,c);
        f = fma(f,r,d);
        Real df = fma(3*a, r, 2*b);
        df = fma(df, r, c);
        if (df != 0) {
            // No standard library feature for fused-divide add!
            r -= f/df;
        }
    }
    std::sort(roots.begin(), roots.end());
    return roots;
}

// Computes the empirical residual p(r) (first element) and expected residual eps*|rp'(r)| (second element) for a root.
// Recall that for a numerically computed root r satisfying r = r_0(1+eps) of a function p, |p(r)| <= eps|rp'(r)|.
template<typename Real>
std::array<Real, 2> cubic_root_residual(Real a, Real b, Real c, Real d, Real root) {
    using std::fma;
    using std::abs;
    std::array<Real, 2> out;
    Real residual = fma(a, root, b);
    residual = fma(residual,root,c);
    residual = fma(residual,root,d);

    out[0] = residual;

    Real expected_residual = fma(3*a, root, 2*b);
    expected_residual = fma(expected_residual, root, c);
    expected_residual = abs(root*expected_residual)*std::numeric_limits<Real>::epsilon();
    out[1] = expected_residual;
    return out;
}

}
#endif


#include <blitzml/smooth_loss/huber_loss.h>
#include <blitzml/base/math_util.h>
#include <math.h>

namespace BlitzML {

value_t HuberLoss::compute_loss(value_t a_dot_omega, value_t label) const {
  value_t residual = a_dot_omega - label;
  if (residual < -1.) {
    return -residual - 0.5;
  } else if (residual > 1.) {
    return residual - 0.5;
  } else {
    return 0.5 * sq(residual);
  }
}

value_t HuberLoss::compute_conjugate(value_t dual_variable,
                                     value_t label) const {
  return dual_variable * label + sq(dual_variable) / 2;
}

value_t HuberLoss::compute_deriative(value_t a_dot_omega,
                                     value_t label) const {
  value_t residual = a_dot_omega - label;
  if (residual < -1.) {
    return -1.;
  } else if (residual > 1.) {
    return 1.;
  } else {
    return residual;
  }
}

value_t HuberLoss::compute_2nd_derivative(value_t a_dot_omega,
                                             value_t label) const {
  value_t residual = a_dot_omega - label;
  if (residual < -1.) {
    return MIN_SMOOTH_LOSS_2ND_DERIVATIVE;
  } else if (residual > 1.) {
    return MIN_SMOOTH_LOSS_2ND_DERIVATIVE;
  } else {
    return 1.;
  }
}

value_t HuberLoss::lipschitz_constant() const {
  return 1;
}

} // namespace BlitzML


#include <catch2/catch.hpp>
#include <Euclid/Geometry/Spectral.h>

#include <string>
#include <vector>
#include <iostream>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <Eigen/Core>
#include <Euclid/Math/Numeric.h>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/IO/OffIO.h>

#include <config.h>

using Kernel = CGAL::Simple_cartesian<double>;
using Point_3 = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;
using Mesh = CGAL::Surface_mesh<Point_3>;

TEST_CASE("Geometry, Spectral", "[geometry][spectral]")
{
    std::string fin(DATA_DIR);
    fin.append("bumpy.off");
    std::vector<double> positions;
    std::vector<int> indices;
    Euclid::read_off<3>(fin, positions, nullptr, &indices, nullptr);
    Mesh mesh;
    Euclid::make_mesh<3>(mesh, positions, indices);

    int nv = positions.size() / 3;
    unsigned k = 20;
    Eigen::VectorXd lambdas1, lambdas2;
    Eigen::MatrixXd phis1, phis2;

    auto n1 = Euclid::spectrum(
        mesh, k, lambdas1, phis1, Euclid::SpecOp::mesh_laplacian);
    auto n2 = Euclid::spectrum(
        mesh, k, lambdas2, phis2, Euclid::SpecOp::graph_laplacian);

    // test size of outputs
    REQUIRE(n1 == k);
    REQUIRE(n2 == k);
    REQUIRE(lambdas1.size() == k);
    REQUIRE(lambdas2.size() == k);
    REQUIRE(phis1.rows() == nv);
    REQUIRE(phis1.cols() == k);
    REQUIRE(phis2.rows() == nv);
    REQUIRE(phis2.cols() == k);

    // the first eigenvalue shoule be 0
    REQUIRE(Euclid::eq_abs_err(lambdas1(0), 0.0, 1e-14));
    REQUIRE(Euclid::eq_abs_err(lambdas2(0), 0.0, 1e-14));

    // the first eigenvector should be ones, but need to be scaled though,
    // so we only test if the entries are equal
    REQUIRE(Euclid::eq_abs_err(
        phis1.col(0).maxCoeff(), phis1.col(0).minCoeff(), 1e-14));
    REQUIRE(Euclid::eq_abs_err(
        phis2.col(0).maxCoeff(), phis2.col(0).minCoeff(), 1e-14));

    // eigenvectors of the mesh laplacian are orthogonal wrt mass weighted
    // inner product
    auto D = Euclid::mass_matrix(mesh);
    auto dot = (phis1.col(1).transpose() * D * phis1.col(10))(0);
    REQUIRE(Euclid::eq_abs_err(dot, 0.0, 1e-14));

    // eigenvectors of the graph laplacian are orthonormal
    REQUIRE(Euclid::eq_abs_err(phis2.col(1).dot(phis2.col(10)), 0.0, 1e-14));
    REQUIRE(
        Euclid::eq_abs_err(phis2.col(1).norm(), phis2.col(10).norm(), 1e-14));
}


#include <vector>
#include <cassert>

#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

// Flag for slope reconstruction type
enum class Slope { Zero, Reconstructed };

//! \breif Implements a piecewise cubic Herite interpolation on equidistant meshes.
class PCHI {
public:
    //! \brief Construct the slopes frome the data, either usinf dinite-differences or assuming f'(x_j) = 0
    //! \param[in] t vector of nodes (assumed equidistant and sorted)
    //! \param[in] y vector of values at nodes t
    //! \param[in] s Flag to set if you want to reconstruct or set slopes to zero
    PCHI(const Eigen::VectorXd & t, const Eigen::VectorXd & y, Slope s = Slope::Reconstructed)
        : t(t), y(y), c(t.size()) {
        // Sanity check
        n = t.size();
        assert( n == y.size() && "t and y must have same dimension." );
        assert( n >= 3 && "need at least two nodes." );
        h = t(1) - t(0);

        switch(s) {
            //// CASE: reconstruction of the slope, assuming f'(x_j) = 0 (O(1))
            case Slope::Zero:
                c= Eigen::VecotrXd::Zero(n);
                break;
            //// CASE: reconstruction of the slope using a second order finite difference (O(h^2))
            case Slope::Reconstructed:
            default:
                c(0) = ( -1*y(2) + 4*y(1) - 3*y(0) ) / 2 / h;
                for(int i = 1; i < n-1; ++i) {
                    c(i) = ( y(i+1) - y(i-1) ) / 2 / h;
                }
//                 c(n-1) = ( y(n-1) - y(n-2) ) / h; // First order
                c(n-1) = ( 3*y(n-1) - 4*y(n-2) + 1*y(n-3) ) / 2 / h;
                break;
                // TODO: reconstruct finite-difference slope
        }
    }

    //! \brief Evaluate the intepolant at the nodes x
    //! Input assumed sorted, unique and inside the interval
    //! \param[in] x vector of points t where to compute s(t)
    //! \return values of interpolant at x (vector)
    Eigen::VectorXd operator() (Eigen::VectorXd x) const {

        Eigen::VectorXd ret(x.size());
        // Stores the current interval index and some temporary variable
        size_t i_star=0:
        double tmp,t1,t2,y1,y2,c1,c2;
        // TODO: evaluate interpolant at x
        for (int j=0; j<x.size();j++){
			t1=t(i_star-1);
			t2=t(i_star);

        return ret;
    }

private:
    // Provided nodes and values (t,y) to compute spline, same size Eigen vectors, c contains slopes
    Eigen::VectorXd t, y, c;
    // Difference t(i)-t(i-1) and coefficients of spline (s'(t_j))
    double h;
    // Size of t, y and c.
    int n;
};

// Interpoland
auto f = [] (double x) { return 1. / (1. + x*x); };

int main() {

    double a = 5; // Interval (-a,a) bounds
    int M = 1000; // Number of  points in which to evaluate

    // Number of subintervals for each test
    std::vector<int> N = {4,8,16,32,64,128,256,512};
    //auto f = [] (double x) {return 1./((1+t)*(1+t));}
    // Precompute values at which evaluate f
    Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(M, -a, a);
    Eigen::VectorXd fx(x.size());
    for(int i = 0; i < x.size(); ++i) {
        fx(i) = f(x(i));
    }

    // TODO: error and rates and print

}

Nummerical Methods for CSE/PS9/piecewise_hermite_interpolation.cpp


// wsn: program to illustrate use of Eigen library to fit a plane to a collection of points

#include<ros/ros.h>

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

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

using namespace Eigen;
using namespace std;
//using namespace Eigen; //if you get tired of typing Eigen:: everywhere, uncomment this.
                         // but I'll leave this as required, for now, to highlight when Eigen classes are being used
    double g_noise_gain = 0.1; //0.1; //0.1; //0.1; //decide how much noise to add to points; start with 0.0, and should get precise results

int main(int argc, char** argv) {
    ros::init(argc, argv, "example_eigen_plane_fit"); //node name
    ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor

    ros::Rate sleep_timer(1.0); //a timer for desired rate, e.g. 1Hz

    //xxxxxxxxxxxxxxxxxx  THIS PART IS JUST TO GENERATE DATA xxxxxxxxxxxxxxxxxxxx
    //xxxxxxxxxxxxxxxxxx  NORMALLY, THIS DATA WOULD COME FROM TOPICS OR FROM GOALS xxxxxxxxx
    // define a plane and generate some points on that plane
    // the plane can be defined in terms of a normal vector and a distance from the origin
    Eigen::Vector3d normal_vec(1,2,3); // here is an arbitrary normal vector, initialized to (1,2,3) upon instantiation
    ROS_INFO("creating example noisy, planar data...");
    cout<<"normal: "<<normal_vec.transpose()<<endl; //.transpose() is so I can display the components on the same line, instead of as a column
    normal_vec/=normal_vec.norm(); // make this vector unit length
    cout<<"unit length normal: "<<normal_vec.transpose()<<endl;
    double dist = 1.23;  // define the plane to have this distance from the origin.  Note that if we care about positive/negative sides of a plane,
                         // then this "distance" could be negative, as measured from the origin to the plane along the positive plane normal
    cout<<"plane distance from origin: "<<dist<<endl;

    // to generate points on the plane, construct a pair of axes perpendicular to the plane normal
    Eigen::Vector3d v1,v2; //need to fill in values for these
    // we'll use an Eigen "Matrix" type to help out.  Define a 3x3 "double precision" matrix, Matrix3d
    //define a rotation about the z axis of 90 deg; elements of this matrix are:
    // [0,-1,0;
    //  1, 0, 0;
    //  0;0;1]
    Eigen::Matrix3d Rot_z;
    Rot_z.row(0)<<0,-1,0;  // populate the first row--shorthand method
    Rot_z.row(1)<<1, 0,0;  //second row
    Rot_z.row(2)<<0, 0,1;  // yada, yada
    cout<<"Rot_z: "<<endl;

    cout<<Rot_z<<endl;  // Eigen matrices and vectors are nicely formatted; better: use ROS_INFO_STREAM() instead of cout

    ROS_INFO_STREAM(endl<<Rot_z);
    // we need another vector to generate two desired vectors in our plane.
    // start by generating a new vector that is a rotation of our normal vector, rotated about the z-axis
    // this hack will NOT work if our normal_vec = [0,0,1],
    v1 = Rot_z*normal_vec; //here is how to multiply a matrix times a vector
        //although Rot_z and normal_vec are both objects (with associated data and member methods), multiplication is defined,
        // resulting in the data members being altered or generated as expected for matrix-vector multiplies
    ROS_INFO_STREAM("v1: "<<v1.transpose()<<endl);

    // let's look at the dot product between v1 and normal_vec, using two approaches
    double dotprod = v1.dot(normal_vec); //using the "dot()" member function
    double dotprod2 = v1.transpose()*normal_vec;// alt: turn v1 into a row vector, then multiply times normal_vec

    cout<<"v1 dot normal: "<<dotprod<<"; v1.transpose()*normal_vec: "<<dotprod2<<endl; //yields the same answer
    cout<<"(should be identical)"<<endl;

    // let's look at the cross product, v1 X normal_vec; use the member fnc cross()
    v2 = v1.cross(normal_vec);
    v2/=v2.norm(); // normalize the output, i.e. make v2 unit length
    cout<<"v2: "<<v2.transpose()<<endl;

    //because v2 was generated from the cross product of v1 and normal_vec, it should be perpendicular to both
    // i.e., the dot product with v1 or normal_vec should be = 0
    dotprod = v2.dot(normal_vec);
    cout<<"v2 dot normal_vec = "<<dotprod<<"  (should be zero)"<<endl;

    v1 = v2.cross(normal_vec);  // re-use v1; make it the cross product of v2 into normal_vec
      // thus, v1 should now also be perpendicular to normal_vec (and thus it is parallel to our plane)
      // and also perpendicular to v2 (so both v1 and v2 lie in the plane and are perpendicular to each other)
    cout<<"v1= "<<v1.transpose()<<endl;
    cout<<" v1 dot v2 = "<<v1.dot(v2)<<"; v1 dot normal_vec = "<<v1.dot(normal_vec)<<endl;
    cout<<"(these should also be zero)"<<endl;
    // we now have two orthogonal vectors, both perpendicular to our defined plane's normal
    // we'll use these to generate some points that lie in our defined plane:

    int npts= 10; // create this many planar points
    Eigen::MatrixXd points_mat(3,npts);  // create a matrix, double-precision values, 3 rows and npts columns
            // we will populate this with 3-D points, column by column
    Eigen::Vector3d point; //a 3x1 vector
    Eigen::Vector2d rand_vec; //a 2x1 vector
    //generate random points that all lie on plane defined by distance and normal_vec
    for (int ipt = 0;ipt<npts;ipt++) {
    	// MatrixXd::Random returns uniform random numbers in the range (-1, 1).
    	rand_vec.setRandom(2,1);  // populate 2x1 vector with random values
    	//cout<<"rand_vec: "<<rand_vec.transpose()<<endl; //optionally, look at these random values
    	//construct a random point ON the plane normal to normal_vec at distance "dist" from origin:
        // a point on the plane is a*x_vec + b*y_vec + c*z_vec, where we may choose
        // x_vec = v1, y_vec = v2 (both of which are parallel to our plane) and z_vec is the plane normal
        // choose coefficients a and b to be random numbers, but "c" must be the plane's distance from the origin, "dist"
    	point =  rand_vec(0)*v1 + rand_vec(1)*v2 + dist*normal_vec;
	//save this point as the i'th column in the matrix "points_mat"
	points_mat.col(ipt) = point;
    }

    //all of the above points are identically on the plane defined by normal_vec and dist

    cout<<"random points on plane (in columns): "<<endl; // display these points; only practical for relatively small number of points
    cout<<points_mat<<endl;

    // add random noise to these points in range [-0.1,0.1]
    Eigen::MatrixXd Noise = Eigen::MatrixXd::Random(3,npts);

    cout<<"noise_gain = "<<g_noise_gain<<"; edit this as desired"<<endl;
    // add two matrices, term by term.  Also, scale all points in a matrix by a scalar: Noise*g_noise_gain
    points_mat = points_mat + Noise*g_noise_gain;
    cout<<"random points on plane (in columns) w/ noise: "<<endl;
    cout<<points_mat<<endl;
    //xxxxxxxxxxxxxxxxxx  DONE CREATING PLANAR DATA xxxxxxxxxxxxxxxxxx
    // xxxxxxxxxxxxxxx   NOW, INTERPRET THE DATA TO DISCOVER THE UNDERLYING PLANE xxxxxxxx

    //now let's see if we can discover the plane from the data:
    cout<<endl<<endl;
    ROS_INFO("starting identification of plane from data: ");
    // first compute the centroid of the data:
    // here's a handy way to initialize data to all zeros; more variants exist
    // see http://eigen.tuxfamily.org/dox/AsciiQuickReference.txt
    Eigen::Vector3d centroid = Eigen::MatrixXd::Zero(3,1);

    //add all the points together:
    npts = points_mat.cols(); // number of points = number of columns in matrix; check the size
    cout<<"matrix has ncols = "<<npts<<endl;
    for (int ipt =0;ipt<npts;ipt++) {
	centroid+= points_mat.col(ipt); //add all the column vectors together
    }
    centroid/=npts; //divide by the number of points to get the centroid
    cout<<"centroid: "<<centroid.transpose()<<endl;

    // subtract this centroid from all points in points_mat:
    Eigen::MatrixXd points_offset_mat = points_mat;
    for (int ipt =0;ipt<npts;ipt++) {
        points_offset_mat.col(ipt)  = points_offset_mat.col(ipt)-centroid;
    }
    //compute the covariance matrix w/rt x,y,z:
    Eigen::Matrix3d CoVar;
    CoVar = points_offset_mat*(points_offset_mat.transpose());  //3xN matrix times Nx3 matrix is 3x3
    cout<<"covariance: "<<endl;
    cout<<CoVar<<endl;

    // here is a more complex object: a solver for eigenvalues/eigenvectors;
    // we will initialize it with our covariance matrix, which will induce computing eval/evec pairs
    Eigen::EigenSolver<Eigen::Matrix3d> es3d(CoVar);

    Eigen::VectorXd evals; //we'll extract the eigenvalues to here
    //cout<<"size of evals: "<<es3d.eigenvalues().size()<<endl;
    //cout<<"rows,cols = "<<es3d.eigenvalues().rows()<<", "<<es3d.eigenvalues().cols()<<endl;
    cout << "The eigenvalues of CoVar are:" << endl << es3d.eigenvalues().transpose() << endl;
    cout<<"(these should be real numbers, and one of them should be zero)"<<endl;
    cout << "The matrix of eigenvectors, V, is:" << endl;
    cout<< es3d.eigenvectors() << endl << endl;
    cout<< "(these should be real-valued vectors)"<<endl;
    // in general, the eigenvalues/eigenvectors can be complex numbers
    //however, since our matrix is self-adjoint (symmetric, positive semi-definite), we expect
    // real-valued evals/evecs;  we'll need to strip off the real parts of the solution

    evals= es3d.eigenvalues().real(); // grab just the real parts
    cout<<"real parts of evals: "<<evals.transpose()<<endl;

    // our solution should correspond to an e-val of zero, which will be the minimum eval
    //  (all other evals for the covariance matrix will be >0)
    // however, the solution does not order the evals, so we'll have to find the one of interest ourselves

    double min_lambda = evals[0]; //initialize the hunt for min eval
    Eigen::Vector3cd complex_vec; // here is a 3x1 vector of double-precision, complex numbers
    Eigen::Vector3d est_plane_normal;
    complex_vec=es3d.eigenvectors().col(0); // here's the first e-vec, corresponding to first e-val
    //cout<<"complex_vec: "<<endl;
    //cout<<complex_vec<<endl;
    est_plane_normal = complex_vec.real();  //strip off the real part
    //cout<<"real part: "<<est_plane_normal.transpose()<<endl;
    //est_plane_normal = es3d.eigenvectors().col(0).real(); // evecs in columns

    double lambda_test;
    int i_normal=0;
    //loop through "all" ("both", in this 3-D case) the rest of the solns, seeking min e-val
    for (int ivec=1;ivec<3;ivec++) {
        lambda_test = evals[ivec];
    	if (lambda_test<min_lambda) {
		min_lambda =lambda_test;
                i_normal= ivec; //this index is closer to index of min eval
		est_plane_normal = es3d.eigenvectors().col(ivec).real();
        }
    }
    // at this point, we have the minimum eval in "min_lambda", and the plane normal
    // (corresponding evec) in "est_plane_normal"/
    // these correspond to the ith entry of i_normal
    cout<<"min eval is "<<min_lambda<<", corresponding to component "<<i_normal<<endl;
    cout<<"corresponding evec (est plane normal): "<<est_plane_normal.transpose()<<endl;
    cout<<"correct answer is: "<<normal_vec.transpose()<<endl;
    double est_dist = est_plane_normal.dot(centroid);
    cout<<"est plane distance from origin = "<<est_dist<<endl;
    cout<<"correct answer is: "<<dist<<endl;
    cout<<endl<<endl;

    //xxxx  one_vec*dist = point.dot(nx,ny,nz)
    // so, one_vec = points_mat.transpose()*x_vec, where x_vec = [nx;ny;nz]/dist (does not work if dist=0)
    // this is of the form: b = A*x, an overdetermined system of eqns
    // solve this using one of many Eigen methods
    // see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html

    ROS_INFO("2ND APPROACH b = A*x SOLN");
    Eigen::VectorXd  ones_vec= Eigen::MatrixXd::Ones(npts,1); // this is our "b" vector in b = A*x
    Eigen::MatrixXd A = points_mat.transpose(); // make this a Nx3 matrix, where points are along the rows
    // we'll pick the "full pivot LU" solution approach; see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
    // a matrix in "Eigen" has member functions that include solution methods to this common problem, b = A*x
    // use: x = A.solution_method(b)
    Eigen::Vector3d x_soln = A.fullPivLu().solve(ones_vec);
    cout<<"x_soln: "<<x_soln.transpose()<<endl;
    double dist_est2 = 1.0/x_soln.norm();
    x_soln*=dist_est2;
    cout<<"normal vec, 2nd approach: "<<x_soln.transpose()<<endl;
    cout<<"plane distance = "<<dist_est2<<endl;

    return 0;

   // while (ros::ok()) {
   //     sleep_timer.sleep();
   // }
}


#ifndef PCP_COMMON_NORMALS_NORMAL_ESTIMATION_HPP
#define PCP_COMMON_NORMALS_NORMAL_ESTIMATION_HPP

/**
 * @file
 * @ingroup common
 */

#include "pcp/common/normals/normal.hpp"
#include "pcp/traits/point_map.hpp"
#include "pcp/traits/point_traits.hpp"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <cstdint>
#include <iterator>

namespace pcp {

/**
 * @ingroup common
 * @brief
 * Estimates the normal from a group of points using PCA.
 * @tparam ForwardIter Type of iterator to the points
 * @tparam PointViewMap Type satisfying PointViewMap concept
 * @tparam Normal Type of the normal to return
 * @param it Begin iterator to the points
 * @param end End iterator to the points
 * @param point_map The point view map property map
 * @return
 */
template <class ForwardIter, class PointViewMap, class Normal = pcp::normal_t>
Normal estimate_normal(ForwardIter it, ForwardIter end, PointViewMap const& point_map)
{
    using normal_type = Normal;

    static_assert(
        traits::is_point_view_map_v<PointViewMap, decltype(*it)>,
        "Type of point_map must satisfy PointViewMap concept");

    auto const n = std::distance(it, end);
    Eigen::Matrix3Xf V;
    V.resize(3, n);
    for (auto i = 0; i < n; ++i, ++it)
    {
        auto p              = point_map(*it);
        V.block(0, i, 3, 1) = Eigen::Vector3f(p.x(), p.y(), p.z());
    }

    Eigen::Vector3f const Mu      = V.rowwise().mean();
    Eigen::Matrix3Xf const Vprime = V.colwise() - Mu;
    Eigen::Matrix3f const Cov     = Vprime * Vprime.transpose();
    Eigen::SelfAdjointEigenSolver<decltype(Cov)> A(Cov);
    auto const l  = A.eigenvalues();
    auto const& X = A.eigenvectors();

    normal_type normal;
    // instead of sorting, just use 3 if statements
    // First eigenvalue is smallest
    if (l(0) <= l(1) && l(0) <= l(2))
    {
        normal = {X(0, 0), X(1, 0), X(2, 0)};
    }
    // Second eigenvalue is smallest
    if (l(1) <= l(0) && l(1) <= l(2))
    {
        normal = {X(0, 1), X(1, 1), X(2, 1)};
    }
    // Third eigenvalue is smallest
    if (l(2) <= l(0) && l(2) <= l(1))
    {
        normal = {X(0, 2), X(1, 2), X(2, 2)};
    }

    // normal is already normalized, since Eigen returns
    // normalized eigenvectors
    return normal;
}

} // namespace pcp

#endif // PCP_COMMON_NORMALS_NORMAL_ESTIMATION_HPP


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#include <cmath>
#include <cassert>

using namespace std;
using Bint = boost::multiprecision::cpp_int;

Bint ilog(Bint n){
    Bint result = ZERO;
    while(n>0){
        n = n/2;
        result++;
    }
    return result;
}

Bint ilog(Bint n, Bint const& a){
    Bint result = ZERO;
    while(n>0){
        n = n/a;
        result++;
    }
    return result;
}

Bint _jacobi_symbol(Bint const& a, Bint const& p){
    if(a==1){ // (11) p43 [Wada 2001]
        return ONE;
    }
    else if(a==-1){ // (14) p43 [Wada 2001]
        if(a%4==1){
            return ONE;
        }
        else{
            return MINUS_ONE;
        }
    }
    else if(a==2){ // (15) p43 [Wada 2001]// (15) p43 [Wada 2001]
        int tmp = (int)(a%8);
        if(tmp == 1 or tmp == 7){
            return ONE;
        }
        else{
            return MINUS_ONE;
        }
    }

    if(a%2==0){ // (13) p43 [Wada 2001]
        return _jacobi_symbol(2, p) * _jacobi_symbol(a/2, p);
    }
    else{ // (16) p43 [Wada 2001]
        if(a%4==1 or p%4==1){
            return _jacobi_symbol(p%a, a);
        }
        else{
            return MINUS_ONE * _jacobi_symbol(p%a, a);
        }
    }
}

Bint jacobi_symbol(Bint const& a, Bint const& p){
    //returns (a/p)
    //https://en.wikipedia.org/wiki/Jacobi_symbol
    assert(p%2==1);
    if(a == 0){
        return ZERO;
    }
    assert(gcd(a,p)==1);
    return _jacobi_symbol(a%p, p);
}

Bint quadratic_residue(Bint const& a, Bint const& p){
    // returns x s.t. x^2 ≡ a (mod p)

}

//return first(smallest) prime p s.t. p>=n
Bint next_prime(Bint const& n){
    if(n<=2){
        return TWO;
    }
    Bint retval = n;
    if(retval%2==1){
        retval++;
    }
    while(true){
        if(boost::multiprecision::miller_rabin_test(retval, 25)){
            return retval;
        }
        retval+=2;
    }
}


#include <vector>
#include <cmath>

#include <math.h>
#include <boost/tuple/tuple.hpp>

#include "gnuplot-iostream.h"

using namespace std;
const int SAMPLES = 1000;
double pi = 3.1415926535897;

struct datas {
public:
	double a;
	double b;
	datas(double x, double y) {
		a = x;
		b = y;
	}
};

datas getVal(double in, int n, int k, int N) {
	double bn = (2 * pi*k*n / N);
	return datas(in*(cos(-bn)),in*sin(-bn));
}

vector<double> dft(vector<double> &in) {
	vector<double> ret;
	int N = in.size();
	for (int i = 1; i < N / 2; i++) {
		double suma = 0;
		double sumb = 0;
		for (int n = 0; n < N; n++) {
			datas d = getVal(in.at(n), n, i, N);
			suma += d.a;
			sumb += d.b;
		}
		double sum = sqrt(pow(suma, 2) + pow(sumb, 2));
		ret.push_back(sum / (N/2));
	}
	return ret;
}
const int range = 20;
int main() {

	Gnuplot gp;

	// Gnuplot vectors (i.e. arrows) require four columns: (x,y,dx,dy)+ sin(3*x) + sin(5*x)
	gp << "set samples " << SAMPLES << "\n";
	gp << "set xrange[0:" << range << "]\n";
	gp << "plot '-' with lines title 'de'\n";
	std::vector<double> xs;
	std::vector<double> samples;
	for (int i = 0; i < SAMPLES; i++) {
		double x = 0 + ((double)range / SAMPLES)*i;
		xs.push_back(x);
		samples.push_back(sin(x) + sin(3 * x) + sin(5 * x));
	}
	gp.send1d(boost::make_tuple(xs,
		samples));
	for (int i = 0; i < SAMPLES / 2; i++) {
		xs.at(i)=i;
	}
	vector<double> dat =dft(samples);

	std::vector<double>::iterator it;
	it = dat.begin();
	dat.insert(it, 0.0);
	xs.resize(SAMPLES / 2 );

	gp << "set terminal qt 1\n";
	gp << "set xrange[0:100]\n";
	gp << "plot '-' with lines title 'ptf'\n";
	gp.send1d(boost::make_tuple(xs,
		dat));
	std::cout << "Press enter to exit" << std::endl;
	std::cin.get();
}


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

// Copyright Paul A. Bristow 2015.
// Copyright Christopher Kormanyos 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing a polynomial approximation of tgamma(negatable).

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <iomanip>
#include <iostream>

#include <boost/array.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/tools/rational.hpp>

namespace local
{
  template<typename NumericType>
  NumericType tgamma(const NumericType& x)
  {
    // This subroutine uses a polynomial approximation to
    // computes tgamma(x - 2) to approximately order 7
    // in the range 2 < x < 3.

    // The coefficients originate from J. F. Hart et al.,
    // Computer Approximations (John Wiley and Sons, Inc., 1968).
    // See Chap. 7, Tables of Coefficients, Table 5206 on page 244.
    BOOST_CONSTEXPR boost::array<NumericType, 8U> coefs =
    {
      NumericType(0.9999999757437L),
      NumericType(0.4227874604607L),
      NumericType(0.4117741970939L),
      NumericType(0.0821117276973L),
      NumericType(0.0721101941645L),
      NumericType(0.00445108786245L),
      NumericType(0.005159029832L),
      NumericType(0.0016063028892L),
    };

    return boost::math::tools::evaluate_polynomial(coefs, x - 2);
  }
}

int main()
{
  // This example performs computations of the tgamma(x)
  // function for x = 1/2 for a fixed-point negatable
  // type and built-in float.

  // A 32-bit fixed-point type is used, as might be
  // well-suited for a high-performance 32-bit embedded
  // system that does not have or does not use an FPU.

  typedef boost::fixed_point::negatable<7, -24> fixed_point_type;
  typedef fixed_point_type::float_type float_point_type;

  // Here we compute tgamma(5/2) and subsequently perform
  // two iterations of downward recursion via division with
  // [(3/2) * (1/2)] = (3/4). The result is tgamma(1/2),
  // which has a known closed-form value = sqrt(pi).

  const fixed_point_type x = fixed_point_type(5U) / 2U;
  const fixed_point_type g = (local::tgamma(x) * 4U) / 3U;

  std::cout << std::setprecision(6)
            << std::fixed
            << g
            << std::endl;

  using std::sqrt;

  // Compare with the control value, sqrt(pi),
  // computed with a built-in floating-point type.
  std::cout << std::setprecision(6)
            << std::fixed
            << sqrt(boost::math::constants::pi<float_point_type>())
            << std::endl;
}


#include <gmp_lib/math/math.h>

#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

namespace as64_
{

namespace gmp_
{

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

Eigen::Vector4d rotm2quat(Eigen::Matrix3d rotm, bool is_rotm_orthonormal)
{
    if (!is_rotm_orthonormal) return mat2quat(rotm);

    Eigen::Quaternion<double> temp_quat(rotm);
    Eigen::Vector4d quat;
    quat << temp_quat.w(), temp_quat.x(), temp_quat.y(), temp_quat.z();

    quat = quat * (2*(quat(0)>=0)-1); // to avoid discontinuities

    return quat;
}

arma::vec rotm2quat(const arma::mat &rotm, bool is_rotm_orthonormal)
{
  arma::vec quat(4);

  Eigen::Map<const Eigen::Matrix3d> rotm_wrapper(rotm.memptr());
  Eigen::Map<Eigen::Vector4d> quat_wrapper(quat.memptr());
  quat_wrapper = rotm2quat(rotm_wrapper, is_rotm_orthonormal);

  return quat;
}

Eigen::Vector4d mat2quat(Eigen::Matrix3d R)
{
  Eigen::Matrix4d K;
  // Calculate all elements of symmetric K matrix
  K(0,0) = R(0,0) - R(1,1) - R(2,2);
  K(0,1) = K(1,0) = R(0,1) + R(1,0);
  K(0,2) = K(2,0) = R(0,2) + R(2,0);
  K(0,3) = K(3,0) = R(2,1) - R(1,2);

  K(1,1) = R(1,1) - R(0,0) - R(2,2);
  K(1,2) = K(2,1) = R(1,2) + R(2,1);
  K(1,3) = K(3,1) = R(0,2) - R(2,0);

  K(2,2) = R(2,2) - R(0,0) - R(1,1);
  K(2,3) = K(3,2) = R(1,0) - R(0,1);

  K(3,3) = R(0,0) + R(1,1) + R(2,2);

  K = K/3;

  // For each input rotation matrix, calculate the corresponding eigenvalues
  // and eigenvectors. The eigenvector corresponding to the largest eigenvalue
  // is the unit quaternion representing the same rotation.

  Eigen::EigenSolver<Eigen::Matrix4d> es(K);

  Eigen::Vector4cd eigVal = es.eigenvalues();
  Eigen::Matrix4d eigVec = es.eigenvectors().real(); // keep only real part
  int maxIdx;
  eigVal.real().maxCoeff(&maxIdx);

  Eigen::Vector4d quat;
  quat << eigVec(3,maxIdx), eigVec(0,maxIdx), eigVec(1,maxIdx), eigVec(2,maxIdx);

  // By convention, always keep scalar quaternion element positive.
  // Note that this does not change the rotation that is represented
  // by the unit quaternion, since q and -q denote the same rotation.
  if (quat(0) < 0) quat = -quat;

  return quat;
}

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

Eigen::Matrix3d quat2rotm(Eigen::Vector4d quat)
{
  double qw=quat(0), qx=quat(1), qy=quat(2), qz=quat(3);

  Eigen::Matrix3d rotm;
  rotm << 1 - 2*qy*qy - 2*qz*qz, 	2*qx*qy - 2*qz*qw, 	2*qx*qz + 2*qy*qw,
	    2*qx*qy + 2*qz*qw, 	      1 - 2*qx*qx - 2*qz*qz, 	2*qy*qz - 2*qx*qw,
	    2*qx*qz - 2*qy*qw, 	        2*qy*qz + 2*qx*qw, 	1 - 2*qx*qx - 2*qy*qy;

  return rotm;
}

arma::mat quat2rotm(const arma::vec &quat)
{
  double qw=quat(0), qx=quat(1), qy=quat(2), qz=quat(3);

  arma::mat rotm;
  rotm = {{1 - 2*qy*qy - 2*qz*qz,      2*qx*qy - 2*qz*qw,      2*qx*qz + 2*qy*qw},
	        {    2*qx*qy + 2*qz*qw,  1 - 2*qx*qx - 2*qz*qz,      2*qy*qz - 2*qx*qw},
	        {    2*qx*qz - 2*qy*qw,      2*qy*qz + 2*qx*qw,  1 - 2*qx*qx - 2*qy*qy}};
  // rotm << 1 - 2*qy*qy - 2*qz*qz << 	2*qx*qy - 2*qz*qw     <<  	2*qx*qz + 2*qy*qw << arma::endr
	//      << 2*qx*qy + 2*qz*qw     <<  1 - 2*qx*qx - 2*qz*qz <<  	2*qy*qz - 2*qx*qw << arma::endr
	//      << 2*qx*qz - 2*qy*qw     <<    2*qy*qz + 2*qx*qw   << 	1 - 2*qx*qx - 2*qy*qy;

  return rotm;
}

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

Eigen::Vector4d rotm2axang(Eigen::Matrix3d rotm)
{
  Eigen::AngleAxis<double> angleAxis(rotm);

  Eigen::Vector4d axang;
  axang(3) = angleAxis.angle();
  axang.segment(0,3) = angleAxis.axis();

  return axang;
}

arma::vec rotm2axang(const arma::mat &rotm)
{
  arma::vec axang(4);

  Eigen::Map<const Eigen::Matrix3d> rotm_wrapper(rotm.memptr());
  Eigen::Map<Eigen::Vector4d> axang_wrapper(axang.memptr());
  axang_wrapper = rotm2axang(rotm_wrapper);

  return axang;
}

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

Eigen::Matrix3d axang2rotm(Eigen::Vector4d axang)
{
  Eigen::Matrix3d rotm;
  Eigen::Vector3d axis = axang.segment(0,3);
  axis /= axis.norm();
  double angle = axang(3);

  double x=axis(0), y=axis(1), z=axis(2), c=std::cos(angle), s=std::sin(angle), t=1-c;
  rotm <<   t*x*x + c,	    t*x*y - z*s,     t*x*z + y*s,
  	        t*x*y + z*s,    t*y*y + c,	     t*y*z - x*s,
            t*x*z - y*s,    t*y*z + x*s,     t*z*z + c;

  return rotm;
}

arma::mat axang2rotm(const arma::vec &axang)
{
  arma::vec axis = axang.subvec(0,2);
  axis /= arma::norm(axis);
  double angle = axang(3);
  double x=axis(0), y=axis(1), z=axis(2), c=std::cos(angle), s=std::sin(angle), t=1-c;

  return { { t*x*x + c,	    t*x*y - z*s,   t*x*z + y*s },
  	       { t*x*y + z*s,   t*y*y + c,	   t*y*z - x*s },
           { t*x*z - y*s,   t*y*z + x*s,   t*z*z + c   } };
}

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

Eigen::Vector4d axang2quat(Eigen::Vector4d axang)
{
  Eigen::Vector4d quat;
  double theta = axang(3);

  quat(0) = std::cos(theta/2);
  quat.segment(1,3) = std::sin(theta/2) * axang.segment(0,3);

  return quat;
}

arma::vec axang2quat(const arma::vec &axang)
{
  arma::vec quat(4);

  Eigen::Map<const Eigen::Vector4d> axang_wrapper(axang.memptr());
  Eigen::Map<Eigen::Vector4d> quat_wrapper(quat.memptr());
  quat_wrapper = axang2quat(axang_wrapper);

  return quat;
}

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

Eigen::Vector4d quat2axang(Eigen::Vector4d quat)
{
  Eigen::Vector4d axang;
  Eigen::Vector3d r = quat.segment(1,3);

  if (r.norm()){
    axang(3) = 2 * std::acos(quat(0));
    axang.segment(0,3) = r/r.norm();
  }else axang << 0, 0, 1, 0;

  return axang;
}

arma::vec quat2axang(const arma::vec &quat)
{
  arma::vec axang(4);

  Eigen::Map<const Eigen::Vector4d> quat_wrapper(quat.memptr());
  Eigen::Map<Eigen::Vector4d> axang_wrapper(axang.memptr());
  axang_wrapper = quat2axang(quat_wrapper);

  return axang;
}

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

} // namespace gmp_

} // namespace as64_


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   PlaneFitAndResidualsTest.hpp
 * Author: Jordan McManus
 */

#ifndef PLANEFITANDRESIDUALSTEST_HPP
#define PLANEFITANDRESIDUALSTEST_HPP

#include "catch.hpp"
#include <cmath>
#include <Eigen/Dense>
#include "../src/math/PlaneFitter.hpp"

TEST_CASE("plane fit and residual test") {

    //create plane params
    //Z = Ax + By + C
    double A = 10;
    double B = 20;
    double C = 30;
    Eigen::Vector3d planeParamsZform(A, B, C);

    // general form: ax + by + cz + d = 0 with (a*a + b*b + c*c = 1)
    Eigen::Vector4d planeParamsGeneralForm;
    PlaneFitter::convertPlaneZform2GeneralForm(planeParamsZform, planeParamsGeneralForm);

    Eigen::Vector3d testZform;
    PlaneFitter::convertPlaneGeneralForm2Zform(planeParamsGeneralForm, testZform);

    double eps = 1e-9;
    REQUIRE(std::abs(testZform(0)-A) < eps);
    REQUIRE(std::abs(testZform(1)-B) < eps);
    REQUIRE(std::abs(testZform(2)-C) < eps);

    //create points on plane

    double minX = 5.0;
    double maxX = 10.0;
    double minY = 5.0;
    double maxY = 10.0;
    int n = 100;

    Eigen::VectorXd x = (0.5*(maxX-minX)*Eigen::VectorXd::Random(n)) + (0.5*(minX+maxX)*Eigen::VectorXd::Ones(n));
    Eigen::VectorXd y = (0.5*(maxY-minY)*Eigen::VectorXd::Random(n)) + (0.5*(minY+maxY)*Eigen::VectorXd::Ones(n));
    Eigen::VectorXd z = A*x + B*y + C*Eigen::VectorXd::Ones(n);

    //add noise
    double variance = 0.01;
    Eigen::VectorXd xnoise = variance*Eigen::VectorXd::Random(n);
    Eigen::VectorXd ynoise = variance*Eigen::VectorXd::Random(n);
    Eigen::VectorXd znoise = variance*Eigen::VectorXd::Random(n);

    Eigen::VectorXd xn = x + xnoise;
    Eigen::VectorXd yn = y + ynoise;
    Eigen::VectorXd zn = z + znoise;

    //fit plane
    Eigen::MatrixXd xyz(n, 3);
    xyz.col(0) = xn;
    xyz.col(1) = yn;
    xyz.col(2) = zn;

    Eigen::Vector4d planeParamsGeneralFormEstimation;
    PlaneFitter::fitPlane(xyz, planeParamsGeneralFormEstimation);

    //verify that original plane parameters are obtained
    REQUIRE((planeParamsGeneralFormEstimation-planeParamsGeneralForm).norm() < 0.01);

    // calculate residual of a point to the plane
    double a = planeParamsGeneralForm(0);
    double b = planeParamsGeneralForm(1);
    double c = planeParamsGeneralForm(2);
    double d = planeParamsGeneralForm(3);
    Eigen::Vector3d unitNormal;
    unitNormal << a,b,c;

    double p1x = 1.0;
    double p1y = 1.0;
    double p1z = (a*p1x + b*p1y + d)*(-1.0/c);
    Eigen::Vector3d pointOnPlane;
    pointOnPlane << p1x, p1y, p1z;

    Eigen::Vector3d pointOutsidePlane = pointOnPlane + unitNormal;

    Eigen::MatrixXd points(1,3);
    points.row(0) = pointOutsidePlane;

    Eigen::VectorXd residuals;
    PlaneFitter::calculatePlaneResidualsFromMatrix(residuals, points, planeParamsGeneralForm);
    REQUIRE(std::abs(residuals(0) - 1) < eps);
}

#endif /* PLANEFITANDRESIDUALSTEST_HPP */


#pragma once
/*
Convex Hull Trick
- y = ax + b が順次追加されつつ，最大値/最小値クエリに答える
- y = c(x - a)^2 + b 型の関数を表す(a, b)たちが順次追加されつつ，最小値クエリに答える
Verify:
CF 1179D https://codeforces.com/contest/1179/submission/59448330
CF 1137E https://codeforces.com/contest/1137/submission/59448399
*/
#include <limits>
#include <set>
#include <utility>
#include <vector>
// CUT begin
// Convex Hull Trick
// Implementation Idea:
// https://github.com/satanic0258/Cpp_snippet/blob/master/src/technique/ConvexHullTrick.cpp
// #include <boost/multiprecision/cpp_int.hpp>
// using mpint = boost::multiprecision::cpp_int;
namespace CHT {
using T_CHT = long long;
static const T_CHT T_MIN = std::numeric_limits<T_CHT>::lowest() + 1;
struct Line {
    T_CHT a, b; // y = ax + b
    mutable std::pair<T_CHT, T_CHT>
        rp; // (numerator, denominator) `x` coordinate of the crossing point with next line
    Line(T_CHT a, T_CHT b) : a(a), b(b), rp(T_MIN, T_MIN) {}
    static std::pair<T_CHT, T_CHT> cross(const Line &ll, const Line &lr) {
        return std::make_pair(ll.b - lr.b, lr.a - ll.a); // `ll.a < lr.a` is assumed implicitly
    }
    bool operator<(const Line &r) const {
        if (b == T_MIN) {
            return r.rp.first == T_MIN ? true : a * r.rp.second < r.rp.first;
        } else if (r.b == T_MIN) {
            return rp.first == T_MIN ? false : !(r.a * rp.second < rp.first);
        } else {
            return a < r.a;
        }
    }
};
template <typename T_MP> struct Lines : std::multiset<Line> {
    bool flg_min; // true iff for minimization
    inline bool isNeedless(iterator itr) {
        if (size() == 1) return false;
        auto nxt = std::next(itr);
        if (itr == begin())
            return itr->a == nxt->a and itr->b <= nxt->b;
        else {
            auto prv = std::prev(itr);
            if (nxt == end())
                return itr->a == prv->a and itr->b <= prv->b;
            else
                return T_MP(prv->b - itr->b) * (nxt->a - itr->a) >=
                       T_MP(itr->b - nxt->b) * (itr->a - prv->a);
        }
    }
    void add_line(T_CHT a, T_CHT b) {
        if (flg_min) a = -a, b = -b;
        auto itr = insert({a, b});
        if (isNeedless(itr))
            erase(itr);
        else {
            while (std::next(itr) != end() and isNeedless(std::next(itr))) {
                erase(std::next(itr));
            }
            while (itr != begin() and isNeedless(std::prev(itr))) { erase(std::prev(itr)); }
            if (std::next(itr) != end()) { itr->rp = CHT::Line::cross(*itr, *std::next(itr)); }
            if (itr != begin()) { std::prev(itr)->rp = CHT::Line::cross(*std::prev(itr), *itr); }
        }
    }
    Lines(bool is_minimizer) : flg_min(is_minimizer) {}
    std::pair<T_CHT, T_CHT> get(T_CHT x) {
        auto itr = lower_bound({x, CHT::T_MIN});
        T_CHT retval = CHT::T_MIN, reta = CHT::T_MIN;
        if (itr != end()) { retval = itr->a * x + itr->b, reta = itr->a; }
        if (itr != begin()) {
            T_CHT tmp = std::prev(itr)->a * x + std::prev(itr)->b;
            if (tmp >= retval) { retval = tmp, reta = std::max(reta, std::prev(itr)->a); }
        }
        return std::make_pair(flg_min ? -retval : retval, flg_min ? -reta : reta);
    }
};
} // namespace CHT

template <typename T_MP> struct ConvexHullTrick {
    using T_CHT = CHT::T_CHT;
    CHT::Lines<T_MP> lines;
    ConvexHullTrick(bool is_minimizer) : lines(is_minimizer) {}
    void add_line(T_CHT a, T_CHT b) { lines.add_line(a, b); } // Add y = ax + b
    std::pair<T_CHT, T_CHT> get(T_CHT x) { return lines.get(x); }
    void add_convex_parabola(T_CHT c, T_CHT a, T_CHT b) {
        add_line(c * a * (-2), c * a * a + b);
    } // Add y = c(x - a)^2 + b
    T_CHT parabola_lower_bound(T_CHT c, T_CHT x) { return lines.get(x).first + c * x * x; }
};


#include <vector>

#include <NTL/ZZ.h>

#include "../src/elements/element.hpp"
#include "../src/algorithms/utils.hpp"

/* Example: using the remainder tree algorithm to compute central trinomial
 * coefficients modulo squared primes.
 *
 * The n'th central trinomial coefficient, denoted a_n is the coefficient
 * of x^n in (1+x+x^2)^n. It is also the number of permutations of n symbols
 * taken from {-1, 0, 1} which sum to 0. The a_n satisfy the following identity:
 * a_p = 1 (mod p) for all primes p, and are given by the following recurrence:
 * a_n = [(2*n - 1)*a_{n-1} + 3*(n - 1)*a_{n-2}]/n, with a_0 = a_1 = 1.
 *
 * Suppose we wish to find primes p for which a_p = 1 (mod p^2) using remainder tree.
 * Let R_1 be the 2x2 matrix with ones in the top row and zeros in the bottom. For n>1, define
 *
 *       [2*n - 1,  n]
 * R_n = [3*(n-1),  0]
 *
 * Now, let M_1 = R_1. For n > 1, define
 *
 *       [a_n, a_{n-1}]
 * M_n = [0,         0]
 *
 * Now observe n! * M_n = (R_n)! Our goal is to find
 * a_2 (mod 4), a_3 (mod 9), a_5 (mod 25)... Using remainder tree,
 * we can find n! * M_n (mod n) for a range of values of n, then divide each by n!
 * However, a similar problem to the Wolstenholme case arises: we cannot divide by n modulo n
 * We will employ a similar solution (see wolstenholme.hpp for more explanation)
 *
 * For primes p, we will use remainder tree to compute (R_p)! (mod p^3), take the
 * upper left entry only, divide it by p, and then reduce modulo p^2.
 * With another remainder tree, separately compute the rest of the denominator, (n-1)! modulo p^2,
 * find its modular inverse, and multiply to get the final result: a_p (mod p^2)
 *
 * For the first remainder tree, let A_0 = Id, and A_n = R_n.
 * The moduli m_n will be n^3 when n is prime, and 1 otherwise.
 * For the second remainder tree, let A_0 = A_1 = 1, and then A_n = n-1. The moduli
 * are now just n^2 when n is prime, and 1 otherwise.
 * Divide each of the upper left entries in the output by their index, then reduce modulo p^2,
 * and multiply by the modular inverse of the corresponding output in the second remainder tree.
 */

/* Like the Kurepa example, we will need to use a matrix of integers for this problem.
 * Thankfully, we still do not have to deal with using polynomials.
 */
using NTL::ZZ;
using NTL::Mat;
//TODO: specialize methods for Elt<Mat<ZZ> >, including modding by ZZ

vector<Elt<Mat<ZZ> > > gen_trinomial_numerator(long lower, long upper) {
    vector<Elt<Mat<ZZ> > > output(upper-lower);

    for(long i = lower; i < upper; i++) {
        Mat<ZZ> M;
        M.SetDims(2,2);

        if (i == 0) {
            M[0][0] = ZZ(1);
            M[0][1] = ZZ(0);
            M[1][0] = ZZ(0);
            M[1][1] = ZZ(1);
            output[i] = Elt<Mat<ZZ> >(M);
        }

        else if (i == 1) {
            M[0][0] = ZZ(1);
            M[0][1] = ZZ(1);
            M[1][0] = ZZ(0);
            M[1][1] = ZZ(0);
            output[i] = Elt<Mat<ZZ> >(M);
        }

        else {
            M[0][0] = ZZ(2*i - 1);
            M[0][1] = ZZ(i);
            M[1][0] = ZZ(3*(i - 1));
            M[1][1] = ZZ(0);
            output[i-lower] = Elt<Mat<ZZ> >(M);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_trinomial_denominator(long lower, long upper){
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; ++i) {
        if(i <= 1){
            output[i] = Elt<ZZ>(1);
        }
        else {
            output[i-lower] = Elt<ZZ> (i - 1);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_second_prime_power(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        NTL::ZZ n(i);
        if(ProbPrime(n)) { //Technically a sieve is faster & more correct, but shouldn't make a big difference.
                            //This is just an example anyway.
            power(n, n, 2);
            output[i-lower] = Elt<ZZ>(n);
        }
        else{
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_third_prime_power(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        ZZ n(i);
        if(ProbPrime(n)) { //Technically a sieve is faster & more correct, but shouldn't make a big difference.
                            //This is just an example anyway.
            power(n, n, 3);
            output[i-lower] = Elt<ZZ>(n);
        }
        else{
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

//TODO: combine the above and actually write a search function that zips the outputs and finds XGCD etc.
//TODO: explain how to modify calculate_factorial and compute V


#include <Eigen/Dense>
#include <iostream>
#include <functional> // for std::function
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

// New version 1.1: using sparse matrices, dropping general nonlinear solver, using error instead of residual

//! \brief Implements a single step of the fixed point iteration $x^{(k+1)} = A(x^{(k)})^{-1} * b$
//! \tparam func type of the lambda function implementing A(x)
//! \tparam Vector type for the vector b, x, x_new $\in \mathbf{R}^2$
//! \param[in] A lambda function implementing A(x)
//! \param[in] b rhs vector $b \in \mathbf{R}^n$
//! \param[in] x previous step $x^{(k)}$
//! \param[out] x_new next step $x^{(k+1)}$
template <class func, class Vector>
void fixed_point_step(func&& A, const Vector & b, const Vector & x, Vector & x_new) {
    // Next step
    auto T = A(x);
    Eigen::SparseLU<Eigen::SparseMatrix<double>> Ax_lu;
    Ax_lu.analyzePattern(T);
    Ax_lu.factorize(T);
    x_new = Ax_lu.solve(b);
}

//! \brief Implements a single step of the Netwon iteration for $x^{(k+1)}$
//! Exploits Sherman-Morrison-Woodbury formula for fast inversion of rank-one modification of a matrix.
//! \tparam func type of the lambda function implementing A(x)
//! \tparam Vector type for the vector b, x, x_new $\in \mathbf{R}^2$
//! \param[in] A lambda function implementing A(x)
//! \param[in] b rhs vector $b \in \mathbf{R}^n$
//! \param[in] x previous step $x^{(k)}$
//! \param[out] x_new next step in Newton iteration $x^{(k+1)}$
template <class func, class Vector>
void newton_step(func&& A, const Vector & b, const Vector & x, Vector & x_new) {
    // Reuse LU decomposition with SMW
    auto T = A(x);
    Eigen::SparseLU<Eigen::SparseMatrix<double>> Ax_lu;
    Ax_lu.analyzePattern(T);
    Ax_lu.factorize(T);
    // Solve a bunch of systems
    auto Axinv_b = Ax_lu.solve(b);
    auto Axinv_x = Ax_lu.solve(x);
    // Next step
    x_new = Axinv_b + Ax_lu.solve(x*x.transpose()*(x-Axinv_b)) / (x.norm() + x.dot(Axinv_x) );
}

int main(void) {
    double eps = 10e-14;
    int max_itr = 100;

    // Define a test vector and test rhs and x0 = b
    int n = 8;
    Eigen::SparseMatrix<double> T(n,n);
    T.reserve(3);
    for(int i = 0; i < n; ++i) {
        if(i > 0) T.insert(i,i-1) = 1;
        T.insert(i,i) = 0;
        if(i < n-1) T.insert(i,i+1) = 1;
    }

    Eigen::VectorXd b = Eigen::VectorXd::Random(n);

    // Define a lambda function implementing A(x)
    // auto = std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd &)>
    auto A = [&T, n] (const Eigen::VectorXd & x) -> Eigen::SparseMatrix<double> & { double nrm = x.norm();
        for(int i = 0; i < n; ++i) { T.coeffRef(i,i) = 3 + nrm; } return T; };

    // Perform convergence study with fixed point iteration
    std::cout << std::endl << "*** Fixed point method ***" << std::endl << std::endl;
    // auto = std::function<Eigen::VectorXd(const Eigen::VectorXd &, Eigen::VectorXd &)>
    auto fix_step = [&A, &b] (const Eigen::VectorXd & x, Eigen::VectorXd & x_new) { fixed_point_step(A, b, x, x_new); };

    auto x = b;
    auto x_new = x;

    for( int itr = 0;; ) { // Forever until break

        // Advance to next step, override x with x_{k+1}
        fix_step(x, x_new);

        // Compute residual
        double r = (x - x_new).norm();

        std::cout << "[Step " << itr << "] Error: " << r << std::endl;

        // Termination conditions
        // If tol reached
        if (r < eps) {
            std::cout << "[CONVERGED] in " << itr << " it. due to err. err = " << r << " < " << eps << "." << std::endl;
            break;
        }
        // If max it reached
        if (++itr >= max_itr) {
            std::cout << "[NOT CONVERGED] due to MAX it. = " << max_itr << " reached, err = " << r << "." << std::endl;
            break;
        }
        x = x_new;
    }

    std::cout << std::endl << "x^*_fix = " << std::endl << x_new << std::endl;

    // Perform convergence study with Newton iteration
    std::cout << std::endl << "*** Newton method ***" << std::endl << std::endl;

    // auto = std::function<Eigen::VectorXd(const Eigen::VectorXd &, Eigen::VectorXd &)>
    auto newt_step = [&A, &b] (const Eigen::VectorXd & x, Eigen::VectorXd & x_new) { newton_step(A, b, x, x_new); };

    x = b;

    for( int itr = 0;; ) { // Forever until break

        // Advance to next step, override x with x_{k+1}
        newt_step(x, x_new);

        // Compute residual
        double r = (x - x_new).norm();

        std::cout << "[Step " << itr << "] Error: " << r << std::endl;

        // Termination conditions
        // If tol reached
        if (r < eps) {
            std::cout << "[CONVERGED] in " << itr << " it. due to err. err = " << r << " < " << eps << "." << std::endl;
            break;
        }
        // If max it reached
        if (++itr >= max_itr) {
            std::cout << "[NOT CONVERGED] due to MAX it. = " << max_itr << " reached, err = " << r << "." << std::endl;
            break;
        }
        x = x_new;
    }

    std::cout << std::endl << "x^*_newt = " << std::endl << x_new << std::endl;
}

Nummerical Methods for CSE/PS5/solutions_ps5/quasilin.cpp


#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <string>
#include <vector>
#include <stdexcept>
#include "atoms.h"

using namespace std;
using namespace Eigen;

vector<string> split(const string &s, char delim) {
    stringstream ss(s);
    string item;
    vector<string> elems;
    while (getline(ss, item, delim)) {
        if (!item.empty()){
            elems.push_back(item);
        }
    }
    return elems;
}

class Molecule{

    public:
        vector<int> atomic_numbers;
        MatrixXd coords;
        int n_atoms = 0;

        explicit Molecule(string xyz_filename){                         // Constructor
                extract_from_xyz_file(xyz_filename);
        }

    void extract_from_xyz_file(string &xyz_filename){
        /************************************************
         *  Set coordinates and atomic numbers from a .xyz file
         ***********************************************/

        string line, item;
        ifstream xyz_file (xyz_filename);

        bool assigned_n_atoms = false;

        vector<float> coord_list;

        // Iterate through the xyz file
        while (getline(xyz_file, line, '\n')){

            // Ignore any blank lines etc.
            if (line.empty()){
                continue;
            }

            // Assign the number of atoms
            if (!assigned_n_atoms){
                n_atoms = stoi(line);
                assigned_n_atoms = true;
                continue;
            }

            vector<string> xyz_items = split(line, ' ');

            atomic_numbers.push_back(stoi(xyz_items[0]));
            for (int i=1; i < 4; i++){
                coord_list.push_back(stod(xyz_items[i]));
            }

        }
        xyz_file.close();

        if (atomic_numbers.size() != n_atoms) {
            cout << atomic_numbers.size() << " " << n_atoms << endl;
            throw runtime_error("Number of atoms not equal to the number declared");
        }

        // Now we can assign the coordinate as a Nx3 matrix, after defining it's shape
        coords.resize(n_atoms, 3);

        for (int i=0; i < coord_list.size(); i++){
            coords(i/3, i%3) = coord_list[i];
        }
    }

    double distance_ij(int i, int j){
        /******************************************************
         *  Calculate the distance between to atoms i and j as
         *  √((x_i - x_j)^2 + (y_i - y_j)^2 + (z_i - z_j)^2)
         *****************************************************/

        return sqrt(pow((coords(i, 0) - coords(j, 0)), 2)
                    + pow((coords(i, 1) - coords(j, 1)), 2)
                    + pow((coords(i, 2) - coords(j, 2)), 2));
        }

    double angle_ijk(int i, int j, int k){
        /******************************************************
        *  Calculate the angle in radians between three atoms
        *  raises a runtime error if any indices are the same
        *
        *         i    k
        *          \  /            j is the mid atom
        *           j
        *
        *                    ( v_ij . v_jk  )
        *          θ = arccos(--------------)
          *                  ( |v_ij||v_jk| )
        *
        * where v_ij and v_jk are unit vectors
        *****************************************************/
        if (i==j | i==k | j==k){
            throw runtime_error("Angle must be calcd. with three different "
                                "indices");
            }

        // Calculate the dot product over the three components of the vector
        double dot_product = 0;

        for (int c=0; c < 3; c++){
            dot_product += (coords(i, c) - coords(j, c)) * (coords(k, c) - coords(j, c));
        }

        return acos(dot_product / (distance_ij(i, j) * distance_ij(j, k)));

        }

    MatrixXd distance_matrix(){
        /************************************************
         *  Calculate the distance matrix (N x N)
         ***********************************************/
        double dist;

        // N x N matrix of zeros
        MatrixXd dist_mat = MatrixXd::Zero(n_atoms, n_atoms);

        for (int i=0; i < n_atoms; i++){
            for (int j=i+1; j < n_atoms; j++){

                dist = distance_ij(i, j);

                // And set the values of the symmetric matrix
                dist_mat(i, j) = dist;
                dist_mat(j, i) = dist;
            }
        }
        return dist_mat;
    }

    void shift_to_com(){
        /************************************************
        *  Shift the molecule so that the center of mass_i
        *  (COM) is centered at the origin
        ***********************************************/
        // Center of mass as a column vector
        Vector3d com = Vector3d::Zero();

        double mass_i;
        double total_mass = 0;

        // COM = Σ_i m_i v_i /  Σ_i m_i
        for (int i=0; i<n_atoms; i++){

            mass_i = atomic_weight(atomic_numbers[i]);
            total_mass += mass_i;

            com += mass_i * coords.row(i);
        }

        // Subtract the COM from each row of the coordinates
        // COM needs transposing into a row vector_
        coords.rowwise() -= com.transpose() / total_mass;
    }

    Vector3d moments_of_inertia(){
        /************************************************
        *  Calculate the moments of inertia as the
        *  eigenvalues of the moment of inertia tensor
        ***********************************************/
        shift_to_com();

        VectorXd atomic_weights(n_atoms);

        for (int i=0; i<n_atoms; i++) atomic_weights(i) = atomic_weight(atomic_numbers[i]);

        auto x = coords.col(0);
        auto y = coords.col(1);
        auto z = coords.col(2);

        VectorXd x_sq = x.array().pow(2);
        VectorXd y_sq = y.array().pow(2);
        VectorXd z_sq = z.array().pow(2);

        Matrix3d I_mat = Matrix3d::Zero();
        // Calculate the diagonal elements
        I_mat(0, 0) = atomic_weights.adjoint() * (y_sq + z_sq);
        I_mat(1, 1) = atomic_weights.adjoint() * (x_sq + z_sq);
        I_mat(2, 2) = atomic_weights.adjoint() * (x_sq + y_sq);

        // Calculate the off diagonals
        I_mat(0, 1 ) = -atomic_weights.adjoint() * (x.cwiseProduct(y));
        I_mat(0, 2 ) = -atomic_weights.adjoint() * (x.cwiseProduct(z));
        I_mat(1, 2 ) = -atomic_weights.adjoint() * (y.cwiseProduct(z));

        // Assign the rest of the bottom left of the symmetric matrix
        I_mat(1, 0) = I_mat(0, 1);
        I_mat(2, 0) = I_mat(0, 2);
        I_mat(2, 1) = I_mat(1, 2);

        EigenSolver<MatrixXd> solver(I_mat);
        return solver.eigenvalues().real();
    }

};

int main(){

    Molecule mol = Molecule("../project1.xyz");
    cout << mol.moments_of_inertia().transpose() << endl;

    return 0;
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * arun.cpp
 *
 *  Created on: Jan 31, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include <Eigen/LU>
#include <Eigen/SVD>

#include <memory>
#include <iostream>
#include "mrob/pc_registration.hpp"

using namespace mrob;
using namespace Eigen;

int PCRegistration::arun(const Ref<const MatX> X, const Ref<const MatX> Y, SE3 &T)
{
    assert(X.cols() == 3  && "PCRegistration::Arun: Incorrect sizing, we expect Nx3");
    assert(X.rows() >= 3  && "PCRegistration::Arun: Incorrect sizing, we expect at least 3 correspondences (not aligned)");
    assert(Y.rows() == X.rows()  && "PCRegistration::Arun: Same number of correspondences");
    uint_t N = X.rows();
    /** Algorithm:
     *  1) calculate centroids cx = sum x_i. cy = sum y_i
     *  2) calculate dispersion from centroids qx = x_i - cx
     *  3) calculate matrix H = sum qx_i * qy_i^T
     *  4) svd decomposition: H = U*D*V'
     *      4.5) look for co-linear solutions, that is 2 of the 3 singular values are equal
     *  5) Calculate the rotation solution R = V*U'
     *      5.5) check for correct solution (det = +1) or reflection (det = -1)
     *      step 5.5 is actually unnecessary IF applying Umeyama technique
     *  6) calculate translation as: t = cy - R * cx
     */
    // We have already asserted in base_T that they are 3xN matrices. (and the same length).

    //std::cout << "X: \n" << X << "\nY:\n" << Y << std::endl;
    // 1) calculate centroids cx = E{x_i}. cy = E{y_i}
    //More efficient than creating a matrix of ones when on Release mode (not is Debug mode)
    Mat13 cxm = X.colwise().sum();
    cxm /= (double)N;
    Mat13 cym = Y.colwise().sum();
    cym /= (double)N;

    // 2)  calculate dispersion from centroids qx = x_i - cx
    MatX qx = X.rowwise() - cxm;
    MatX qy = Y.rowwise() - cym;

    // 3) calculate matrix H = sum qx_i * qy_i^T (noting that we are obtaingin row vectors)
    Mat3 H = qx.transpose() * qy;

    // 4) svd decomposition: H = U*D*V'
    JacobiSVD<Matrix3d> SVD(H, ComputeFullU | ComputeFullV);//Full matrices indicate Square matrices

    //test: prints results so far
    /*std::cout << "Checking matrix SVD: \n" << SVD.singularValues() <<
                 ",\n U = " << SVD.matrixU() <<
                 ",\n V = " << SVD.matrixV() << std::endl;*/

    // 4.5) look for co-linear solutions, that is 2 of the 3 singular values are equal
    double l_prev = SVD.singularValues()(0), l;
    for(int i =1; i < 3; ++i)
    {
        l = SVD.singularValues()(i);
        if (fabs(l - l_prev) < 1e-6)

            return 0; //they are co-linear, there exist infinite transformations
        else
            l_prev = l;//this works because we assume that they singular values are ordered.
    }

    // 5) Calculate the rotation solution R = V*U'
    Mat3 R = SVD.matrixV() * SVD.matrixU().transpose();

    // 5.5) check for correct solution (det = +1) or reflection (det = -1)
    // that is, solve the problem for co-planar set of points and centroid, when is l1 > l2 > l3 = 0
    // Since H = D1*u1*v1' + D2*u2*v2' + D3*u3*v3',    and D3 = 0, we can swap signs in V
    // such as Vp = [v1,v2,-v3] and the solution is still minimal, but we want a valid rotation R \in SO(3)
    if (R.determinant() < 0.0 )
    {
        Mat3 Vn;
        Vn << SVD.matrixV().topLeftCorner<3,2>(), -SVD.matrixV().topRightCorner<3,1>();
        R << Vn * SVD.matrixU().transpose();
        //std::cout << "R value = " << R << std::endl;
    }

    // 6) calculate translation as: t = cy - R * cx
    Mat31 t = cym.transpose() - R*cxm.transpose();
    //std::cout << "t = " << t << std::endl;

    // 7) return result
    T.ref2T() << R, t,
                 0,0,0,1;

    return 1;
}


#ifndef _PFASST_QUADRATURE_HPP_
#define _PFASST_QUADRATURE_HPP_

#include <algorithm>
#include <cmath>
#include <complex>
#include <limits>
#include <vector>

#include <Eigen/Dense>

template<typename coeff>
using Matrix = Eigen::Matrix<coeff, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

#include <boost/math/constants/constants.hpp>
using namespace boost::math::constants;

using std::complex;
using std::string;
using std::vector;

#include "interfaces.hpp"

namespace pfasst
{
  template<typename CoeffT>
  class Polynomial
  {
      vector<CoeffT> c;

    public:
      Polynomial(size_t n)
        : c(n)
      {
        fill(c.begin(), c.end(), 0.0);
      }

      size_t order() const
      {
        return c.size() - 1;
      }

      CoeffT& operator[](const size_t i)
      {
        return c.at(i);
      }

      Polynomial<CoeffT> differentiate() const
      {
        Polynomial<CoeffT> p(c.size() - 1);
        for (size_t j = 1; j < c.size(); j++) {
          p[j - 1] = j * c[j];
        }
        return p;
      }

      Polynomial<CoeffT> integrate() const
      {
        Polynomial<CoeffT> p(c.size() + 1);
        for (size_t j = 0; j < c.size(); j++) {
          p[j + 1] = c[j] / (j + 1);
        }
        return p;
      }

      template<typename xtype>
      xtype evaluate(const xtype x) const
      {
        int n = c.size() - 1;
        xtype v = c[n];
        for (int j = n - 1; j >= 0; j--) {
          v = x * v + c[j];
        }
        return v;
      }

      Polynomial<CoeffT> normalize() const
      {
        Polynomial<CoeffT> p(c.size());
        for (size_t j = 0; j < c.size(); j++) {
          p[j] = c[j] / c.back();
        }
        return p;
      }

      vector<CoeffT> roots() const
      {
        assert(c.size() >= 1);
        size_t n = c.size() - 1;

        // initial guess
        Polynomial<complex<CoeffT>> z0(n), z1(n);
        for (size_t j = 0; j < n; j++) {
          z0[j] = pow(complex<double>(0.4, 0.9), j);
          z1[j] = z0[j];
        }

        // durand-kerner-weierstrass iterations
        Polynomial<CoeffT> p = normalize();
        for (size_t k = 0; k < 100; k++) {
          complex<CoeffT> num, den;
          for (size_t i = 0; i < n; i++) {
            num = p.evaluate(z0[i]);
            den = 1.0;
            for (size_t j = 0; j < n; j++) {
              if (j == i) { continue; }
              den = den * (z0[i] - z0[j]);
            }
            z0[i] = z0[i] - num / den;
          }

          // converged?
          CoeffT acc = 0.0;
          for (size_t j = 0; j < n; j++) { acc += abs(z0[j] - z1[j]); }
          if (acc < 2 * std::numeric_limits<CoeffT>::epsilon()) { break; }

          z1 = z0;
        }

        vector<CoeffT> roots(n);
        for (size_t j = 0; j < n; j++) {
          roots[j] = (abs(z0[j]) < 4 * std::numeric_limits<CoeffT>::epsilon()) ? 0.0 : real(z0[j]);
        }

        sort(roots.begin(), roots.end());
        return roots;
      }

      static Polynomial<CoeffT> legendre(const size_t order)
      {
        if (order == 0) {
          Polynomial<CoeffT> p(1);
          p[0] = 1.0;
          return p;
        }

        if (order == 1) {
          Polynomial<CoeffT> p(2);
          p[0] = 0.0;
          p[1] = 1.0;
          return p;
        }

        Polynomial<CoeffT> p0(order + 1), p1(order + 1), p2(order + 1);
        p0[0] = 1.0; p1[1] = 1.0;

        // (n + 1) P_{n+1} = (2n + 1) x P_{n} - n P_{n-1}
        for (size_t m = 1; m < order; m++) {
          for (size_t j = 1; j < order + 1; j++) {
            p2[j] = ((2 * m + 1) * p1[j - 1] - m * p0[j]) / (m + 1);
          }
          p2[0] = - int(m) * p0[0] / (m + 1);

          for (size_t j = 0; j < order + 1; j++) {
            p0[j] = p1[j];
            p1[j] = p2[j];
          }
        }

        return p2;
      }
  };

  enum class QuadratureType {
      GaussLegendre
    , GaussLobatto
    , GaussRadau
    , ClenshawCurtis
    , Uniform
  };

  template<typename node = time_precision>
  vector<node> compute_nodes(size_t nnodes, QuadratureType qtype)
  {
    vector<node> nodes(nnodes);

    if (qtype == QuadratureType::GaussLegendre) {
      auto roots = Polynomial<node>::legendre(nnodes).roots();
      for (size_t j = 0; j < nnodes; j++) {
        nodes[j] = 0.5 * (1.0 + roots[j]);
      }

    } else if (qtype == QuadratureType::GaussLobatto) {
      auto roots = Polynomial<node>::legendre(nnodes - 1).differentiate().roots();
      assert(nnodes >= 2);
      for (size_t j = 0; j < nnodes - 2; j++) {
        nodes[j + 1] = 0.5 * (1.0 + roots[j]);
      }
      nodes.front() = 0.0;
      nodes.back() = 1.0;

    } else if (qtype == QuadratureType::GaussRadau) {
      auto l   = Polynomial<node>::legendre(nnodes);
      auto lm1 = Polynomial<node>::legendre(nnodes - 1);
      for (size_t i = 0; i < nnodes; i++) {
        l[i] += lm1[i];
      }
      auto roots = l.roots();
      for (size_t j = 1; j < nnodes; j++) {
        nodes[j - 1] = 0.5 * (1.0 - roots[nnodes - j]);
      }
      nodes.back() = 1.0;

    } else if (qtype == QuadratureType::ClenshawCurtis) {
      for (size_t j = 0; j < nnodes; j++) {
        nodes[j] = 0.5 * (1.0 - cos(j * pi<node>() / (nnodes - 1)));
      }

    } else if (qtype == QuadratureType::Uniform) {
      for (size_t j = 0; j < nnodes; j++) {
        nodes[j] = node(j) / (nnodes - 1);
      }

    } else {
      throw ValueError("invalid node type passed to compute_nodes.");
    }

    return nodes;
  }

  template<typename node>
  auto augment_nodes(vector<node> const orig) -> pair<vector<node>, vector<bool>> {
    vector<node> nodes = orig;

    bool left = nodes.front() == node(0.0);
    bool right = nodes.back() == node(1.0);

    if (!left)  { nodes.insert(nodes.begin(), node(0.0)); }
    if (!right) { nodes.insert(nodes.end(),   node(1.0)); }

    vector<bool> is_proper(nodes.size(), true);
    is_proper.front() = left;
    is_proper.back() = right;

    return pair<vector<node>, vector<bool>>(nodes, is_proper);
  }

//  enum class QuadratureMatrix { S, Q, QQ }; // returning QQ might be cool for 2nd-order stuff
  enum class QuadratureMatrix { S, Q };

  template<typename node = time_precision>
  Matrix<node> compute_quadrature(vector<node> dst, vector<node> src, vector<bool> is_proper,
                                  QuadratureMatrix type)
  {
    const size_t ndst = dst.size();
    const size_t nsrc = src.size();

    assert(ndst >= 1);
    Matrix<node> mat(ndst - 1, nsrc);
    mat.fill(0.0);

    Polynomial<node> p(nsrc + 1), p1(nsrc + 1);

    for (size_t i = 0; i < nsrc; i++) {
      if (!is_proper[i]) { continue; }

      // construct interpolating polynomial coefficients
      p[0] = 1.0;
      for (size_t j = 1; j < nsrc + 1; j++) { p[j] = 0.0; }
      for (size_t m = 0; m < nsrc; m++) {
        if ((!is_proper[m]) || (m == i)) { continue; }

        // p_{m+1}(x) = (x - x_j) * p_m(x)
        p1[0] = 0.0;
        for (size_t j = 0; j < nsrc;   j++) { p1[j + 1]  = p[j]; }
        for (size_t j = 0; j < nsrc + 1; j++) { p1[j]   -= p[j] * src[m]; }
        for (size_t j = 0; j < nsrc + 1; j++) { p[j] = p1[j]; }
      }

      // evaluate integrals
      auto den = p.evaluate(src[i]);
      auto P = p.integrate();
      for (size_t j = 1; j < ndst; j++) {
        node q = 0.0;
        if (type == QuadratureMatrix::S) {
          q = P.evaluate(dst[j]) - P.evaluate(dst[j - 1]);
        } else if (type == QuadratureMatrix::Q) {
          q = P.evaluate(dst[j]) - P.evaluate(0.0);
        } else {
          throw ValueError("Further matrix types are not implemented yet");
        }

        mat(j - 1, i) = q / den;
      }
    }

    return mat;
  }

  template<typename node = time_precision>
  Matrix<node> compute_interp(vector<node> dst, vector<node> src)
  {
    const size_t ndst = dst.size();
    const size_t nsrc = src.size();

    Matrix<node> mat(ndst, nsrc);

    for (size_t i = 0; i < ndst; i++) {
      for (size_t j = 0; j < nsrc; j++) {
        node den = 1.0;
        node num = 1.0;

        for (size_t k = 0; k < nsrc; k++) {
          if (k == j) { continue; }
          den *= src[j] - src[k];
          num *= dst[i] - src[k];
        }

        if (abs(num) > 1e-32) {
          mat(i, j) = num / den;
        } else {
          mat(i, j) = 0.0;
        }
      }
    }

    return mat;
  }

}  // ::pfasst

#endif