//
// Created by mmath on 7/7/17.
//

#include <cmath>
#include <deque>
#include <algorithm>
#include <functional>
#include <tuple>
#include <iostream>

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

#include "Utilities.hpp"
#include "FunctionApprox.hpp"

namespace pyscan {

    std::ostream& operator<<(std::ostream& object, Vec3 const& v1) {
        object << "(" << v1[0] << ", " << v1[1] << ", " << v1[2] << ")";
        return object;
    }

    bool almostEqualRelative(double A, double B, double maxRelDiff) {
        // Calculate the difference.
        double diff = fabs(A - B);
        A = fabs(A);
        B = fabs(B);
        // Find the largest
        double largest = (B > A) ? B : A;
        return diff <= largest * maxRelDiff;
    }

    bool lineIntersection(Vec2 const& dir1, double d1, Vec2 const& dir2, double d2, Vec2& v_ext) {
        /*
         * Need to test two cases.
         *
         */
        if (almostEqualRelative(dir1[0] * dir2[1], dir1[1] * dir2[0], 8 * std::numeric_limits<double>::epsilon())) {
            return false;
        }

        double denom = 1 / (dir1[0] * dir2[1] - dir1[1] * dir2[0]);

        v_ext[0] = (dir2[1] * d1 - dir1[1] * d2) * denom;
        v_ext[1] = (dir1[0] * d2 - dir2[0] * d1) * denom;
        return true;
    }

    double det3(Vec3 const& dir1, Vec3 const& dir2, Vec3 const& dir3) {
        return dir1[0] * util::det2(dir2[1], dir3[1], dir2[2], dir3[2])
               - dir1[1] * util::det2(dir2[0], dir3[0], dir2[2], dir3[2])
               + dir1[2] * util::det2(dir2[0], dir3[0], dir2[1], dir3[1]);
    }

    using namespace boost::numeric::ublas;

    matrix<double> inverse3x3(matrix<double> const& m) {
        double det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
                     m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
                     m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));

        double invdet = 1 / det;

        matrix<double> minv(3, 3);
        minv(0, 0) = (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) * invdet;
        minv(0, 1) = (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2)) * invdet;
        minv(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * invdet;
        minv(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) * invdet;
        minv(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * invdet;
        minv(1, 2) = (m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2)) * invdet;
        minv(2, 0) = (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * invdet;
        minv(2, 1) = (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) * invdet;
        minv(2, 2) = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * invdet;

        return minv;
    }

    bool lineIntersection(Vec3 const& dir1, Vec3 const& p1,
                          Vec3 const& dir2, Vec3 const& p2,
                          Vec3 const& dir3, Vec3 const& p3,
                          Vec3& v_ext) {

        double d1 = dot(dir1, p1);
        double d2 = dot(dir2, p2);
        double d3 = dot(dir3, p3);
        //equation will be of the form.
        // a1 x + b1 y + c1 z = d1
        // a2 x + b2 y + c2 z = d2
        // a3 x + b3 y + c3 z = d3

        // Compute the rank of the system and see if it is singular
        double dval = det3(dir1, dir2, dir3);
        if (util::aeq(fabs(dval), 0)) {
            return false;
        }

        matrix<double> A(3, 3);

        A(0, 0) = dir1[0], A(0, 1) = dir1[1], A(0, 2) = dir1[2],
        A(1, 0) = dir2[0], A(1, 1) = dir2[1], A(1, 2) = dir2[2],
        A(2, 0) = dir3[0], A(2, 1) = dir3[1], A(2, 2) = dir3[2];

        vector<double> v(3);
        v[0] = d1, v[1] = d2, v[2] = d3;
        auto m = inverse3x3(A);

        v_ext[0] = m(0, 0) * v[0] + m(0, 1) * v[1] + m(0, 2) * v[2];
        v_ext[1] = m(1, 0) * v[0] + m(1, 1) * v[1] + m(1, 2) * v[2];
        v_ext[2] = m(2, 0) * v[0] + m(2, 1) * v[1] + m(2, 2) * v[2];
        return true;
    }

    bool inside(Vec2 const& pos, double alpha, double rho) {
      return (alpha <= pos[0]) && (pos[0] <= 1 - alpha) &&
        (rho <= pos[1]) && (pos[1] <= 1 - rho);
    }

    bool inRange(double a, double b) {
      return (b <= a) && (a <= 1 - b);
    }

    void lineIntersectionI(Vec2 const& dir1, double d1, Vec2 const& dir2, double d2, Vec2& v_ext) {
      /*
        If there is numerical instability then we project to infinity.
      */
      if (!lineIntersection(dir1, d1, dir2, d2, v_ext)) {
        v_ext[0] = std::numeric_limits<double>::infinity();
        v_ext[1] = std::numeric_limits<double>::infinity();
      }
    }

    Vec2 projectToBoundary(Vec2 const& pos, Vec2 const& dir, double alpha, double rho) {
      /*
        Takes the pos and projects it to the rectangle defined by
        [rho, 1 rho] x [alpha, 1 - alpha]
      */
      if (inside(pos, alpha, rho))
        return pos;

      Vec2 rho_b, rho_t, alpha_l, alpha_r;
      double d1 = dot(pos, dir);
      lineIntersectionI(dir, d1, Vec2{0.0, 1.0}, rho, rho_b);
      lineIntersectionI(dir, d1, Vec2{0.0, 1.0}, 1 - rho, rho_t);
      lineIntersectionI(dir, d1, Vec2{1.0, 0.0}, alpha, alpha_l);
      lineIntersectionI(dir, d1, Vec2{1.0, 0.0}, 1 - alpha, alpha_r);
      if (pos[1] < rho && inRange(rho_b[0], alpha))
          return rho_b;
      else if (1 - rho < pos[1] && inRange(rho_t[0], alpha))
        return rho_t;
      else if (pos[0] < alpha && inRange(alpha_l[1], rho))
        return alpha_l;
      else if (1 - alpha < pos[0] && inRange(alpha_r[1], rho))
        return alpha_r;
      // lives in the corner and no good projections
      else if (pos[0] < alpha) {
        if (pos[1] < rho)
          return {alpha, rho};
        else
          return {alpha, 1 - rho};
      } else {
        if (pos[1] < rho)
          return {1 - alpha, rho};
        else
          return {1 - alpha, 1 - rho};
      }
    }

    double approximateHull(double eps,
                           Vec2 const& cc, Vec2 const& cl,
                           std::function<double(Vec2)> phi, //function to maximize
                           std::function<Vec2(Vec2)> lineMaxExt) {

        auto lineMaxF = [&] (Vec2 v1) {
          auto pt = lineMaxExt(v1);
          //std::cout << "line_max = " << pt << std::endl;
          return pt; // projectToBoundary(pt, v1, alpha, rho);
        };

        auto avg = [&] (Vec2 const& v1, Vec2 const& v2) {
            Vec2 v_out = v1 + v2;
            Vec2 tmp;
            double norm = 1.0 / sqrt(v_out[0] * v_out[0] + v_out[1] * v_out[1]);
            tmp[0] = v_out[0] * norm;
            tmp[1] = v_out[1] * norm;
            return tmp;
        };

        struct Frame {
            Vec2 d_cc, d_cl, p_cc, p_cl;
            Frame(Vec2 const& di, Vec2 const& dj, Vec2 const& cc, Vec2 const& cl) :
                    d_cc(di), d_cl(dj), p_cc(cc), p_cl(cl) {}
        };
        double maxRValue = 0;

        std::deque<Frame> frameStack;
        // TODO double check debug to see if there is an issue with an infinite singularity here. Might need to change start.
        //This start needs to be fixed. Compute the mi, bi, and everything explicitly
        frameStack.emplace_back(cc, cl, lineMaxF(cc), lineMaxF(cl));

//#ifndef NDEBUG
//        size_t iter_count = 0;
//#endif
        while(!frameStack.empty()) {
            Frame lf = frameStack.front();

//#ifndef NDEBUG
//            std::cout << "considering triangle" << std::endl;
//            std::cout << lf.d_cc << " " << lf.p_cc << " " << lf.d_cl << " " << lf.p_cl << std::endl;
//#endif
            frameStack.pop_front();
            double di = dot(lf.d_cc, lf.p_cc);
            double dj = dot(lf.d_cl, lf.p_cl);
            double vi = phi(lf.p_cc);
            double vj = phi(lf.p_cl);
            maxRValue = std::max({vi, vj, maxRValue});
            Vec2 p_ext;

            if (lineIntersection(lf.d_cc, di, lf.d_cl, dj, p_ext)) {
                double vw = phi(p_ext);
//#ifndef NDEBUG
//                std::cout << lf.p_cc << " " << vi << std::endl;
//                std::cout << p_ext << " " << vw << std::endl;
//                std::cout << lf.p_cl << " " << vj << std::endl;
//#endif
                //dist(lf)
                if (vw - maxRValue > eps) {

                    //This triangle is worth evaluating
//#ifndef NDEBUG
//                    std::cout << "evaluating triangle" << std::endl;
//#endif
                    Vec2 m_vec = avg(lf.d_cc, lf.d_cl);
                    auto line_max = lineMaxF(m_vec);

                    frameStack.emplace_back(lf.d_cc, m_vec, lf.p_cc, line_max);
                    frameStack.emplace_back(m_vec, lf.d_cl, line_max, lf.p_cl);
                }
            }
//#ifndef NDEBUG
//            iter_count += 1;
//            std::cout << std::endl;
//#endif
        }
//#ifndef NDEBUG
//        std::cout << "maxRValue = " << maxRValue << std::endl;
//        std::cout << iter_count << std::endl;
//#endif
        return maxRValue;
    }

    double approximateHull(double eps,
                           std::function<double(Vec2)> phi, //function to maximize
                           std::function<Vec2(Vec2)> lineMaxF) {
        return std::max(approximateHull(eps, Vec2{1, 0}, Vec2{0, -1}, phi, lineMaxF),
                        approximateHull(eps, Vec2{-1, 0}, Vec2{0, 1}, phi, lineMaxF));
    }

    /*
     * Computes the height of a triangle that has corner points p1, p2, and pt where pt is the top corner.
     * p1, p2 -- corners of the base of the triangle
     * pt -- top corner of the triangle
     * return -- the height of the triangle as a double.
     */
    double height(Vec2 const& p1, Vec2 const& p2, Vec2 const& pt) {
        double a = sqrt((p1[0] - p2[0]) *(p1[0] - p2[0]) + (p1[1] - p2[1]) *(p1[1] - p2[1]));
        double b = sqrt((p1[0] - pt[0]) *(p1[0] - pt[0]) + (p1[1] - pt[1]) *(p1[1] - pt[1]));
        double c = sqrt((p2[0] - pt[0]) *(p2[0] - pt[0]) + (p2[1] - pt[1]) *(p2[1] - pt[1]));
        double s = (a + b + c)/ 2;
        return 2 * sqrt(s * (s - a) * (s - b) * (s - c)) / a;
    }

    std::vector<Vec2> eps_core_set(double eps,
                                   Vec2 const& cc, Vec2 const& cl,
                                   std::function<Vec2(Vec2)> lineMaxF) {

            auto avg = [&] (Vec2 const& v1, Vec2 const& v2) {
                Vec2 v_out = v1 + v2;
                Vec2 tmp;
                double norm = 1.0 / sqrt(v_out[0] * v_out[0] + v_out[1] * v_out[1]);
                tmp[0] = v_out[0] * norm;
                tmp[1] = v_out[1] * norm;
                return tmp;
            };

            struct Frame {
                Vec2 d_cc, d_cl, p_cc, p_cl;
                Frame(Vec2 const& di, Vec2 const& dj, Vec2 const& cc, Vec2 const& cl) :
                        d_cc(di), d_cl(dj), p_cc(cc), p_cl(cl) {}
            };
            auto pcc = lineMaxF(cc), pcl = lineMaxF(cl);
            std::vector<Vec2> pts{ pcc, pcl };
            std::deque<Frame> frameStack;
            frameStack.emplace_back(cc, cl, pcc, pcl);
            while(!frameStack.empty()) {
                Frame lf = frameStack.front();
                frameStack.pop_front();
                double di = dot(lf.d_cc, lf.p_cc);
                double dj = dot(lf.d_cl, lf.p_cl);

                Vec2 p_ext;

                if (lineIntersection(lf.d_cc, di, lf.d_cl, dj, p_ext)) {
                    if (height(lf.p_cc, lf.p_cl, p_ext) > eps) {

                        Vec2 m_vec = avg(lf.d_cc, lf.d_cl);
                        auto line_max = lineMaxF(m_vec);
                        pts.push_back(line_max);
                        frameStack.emplace_back(lf.d_cc, m_vec, lf.p_cc, line_max);
                        frameStack.emplace_back(m_vec, lf.d_cl, line_max, lf.p_cl);
                    }
                }
            }
            return pts;
    }

    std::vector<Vec2> eps_core_set(double eps,
                        std::function<Vec2(Vec2)> lineMaxF) {
        auto core_set1 = eps_core_set(eps, Vec2{1, 0}, Vec2{0, -1},  lineMaxF)
            ,core_set2 = eps_core_set(eps, Vec2{0, 1}, Vec2{1, 0}, lineMaxF)
            ,core_set3 = eps_core_set(eps, Vec2{-1, 0}, Vec2{0, 1}, lineMaxF)
            ,core_set4 = eps_core_set(eps, Vec2{0, -1}, Vec2{-1, 0}, lineMaxF);
        core_set1.insert(core_set1.end(), core_set2.begin(), core_set2.end());
        core_set1.insert(core_set1.end(), core_set3.begin(), core_set3.end());
        core_set1.insert(core_set1.end(), core_set4.begin(), core_set4.end());
        return core_set1;
    }

    point_list_t approx_hull(point_list_t const &pts, double eps) {
        auto max_f = [&] (Vec2 direction) {
            double max_dir = -std::numeric_limits<double>::infinity();
            pt2_t curr_pt(0.0, 0.0, 0.0);
            for (auto& pt : pts) {
                double curr_dir = direction[0] * pt(0) + direction[1] * pt(1);
                if (max_dir < curr_dir) {
                    max_dir = curr_dir;
                    curr_pt = pt;
                }
            }
            return Vec2{curr_pt(0), curr_pt(1)};
        };
        std::vector<pyscan::Point<>> core_set_pts;
        {
            auto vecs = eps_core_set(eps, max_f);
            for (auto &v :vecs) {
                core_set_pts.emplace_back(v[0], v[1], 1.0);
            }
        }
        remove_duplicates(core_set_pts);
        return core_set_pts;
    }

  }


#include "TrICP.h"

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

using namespace navtypes;

struct PointPair // NOLINT(cppcoreguidelines-pro-type-member-init)
{
	point_t mapPoint;
	point_t samplePoint;
	double dist;
};

void heapify(PointPair arr[], int len, int i) {
	int smallest = i;
	int l = 2 * i + 1;
	int r = 2 * i + 2;
	if (l < len && arr[l].dist < arr[smallest].dist) {
		smallest = l;
	}
	if (r < len && arr[r].dist < arr[smallest].dist) {
		smallest = r;
	}

	if (smallest != i) {
		std::swap(arr[i], arr[smallest]);
		heapify(arr, len, smallest);
	}
}

// get some of the point pairs with the least distance using heapsort
std::vector<PointPair> getMinN(PointPair* arr, int len, int minNum) {
	for (int i = len / 2 - 1; i >= 0; i--) {
		heapify(arr, len, i);
	}

	std::vector<PointPair> ret;
	for (int i = len - 1; i > len - 1 - minNum; i--) {
		ret.push_back(arr[0]);
		std::swap(arr[0], arr[i]);
		heapify(arr, i, 0);
	}

	return ret;
}

transform_t computeTransformation(const std::vector<PointPair>& pairs) {
	/*
	 * We need to find a rigid transformation that maps points in the sample to points in
	 * the map. This transformation is not a regular affine, as only rotations and translations
	 * are permitted.
	 *
	 * We can do this with the Kabsch algorithm.
	 * https://en.wikipedia.org/wiki/Kabsch_algorithm
	 */
	Eigen::Vector2d mapCentroid = Eigen::Vector2d::Zero();
	Eigen::Vector2d sampleCentroid = Eigen::Vector2d::Zero();

	for (const PointPair& pair : pairs) {
		mapCentroid += pair.mapPoint.topRows<2>();
		sampleCentroid += pair.samplePoint.topRows<2>();
	}

	mapCentroid /= pairs.size();
	sampleCentroid /= pairs.size();

	int size = pairs.size();
	Eigen::Matrix<double, Eigen::Dynamic, 2> P(size, 2);
	Eigen::Matrix<double, Eigen::Dynamic, 2> Q(size, 2);
	for (int i = 0; i < size; i++) {
		P.row(i) = pairs[i].samplePoint.topRows<2>() - sampleCentroid;
		Q.row(i) = pairs[i].mapPoint.topRows<2>() - mapCentroid;
	}

	Eigen::Matrix2d H = P.transpose() * Q;

	// computing SVD for square matrices, so no QR preconditioner needed
	Eigen::JacobiSVD<Eigen::Matrix2d, Eigen::NoQRPreconditioner> svd;
	svd.compute(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

	Eigen::Matrix2d U = svd.matrixU();
	Eigen::Matrix2d V = svd.matrixV();
	double d = (V * U.transpose()).determinant() > 0 ? 1 : -1;
	Eigen::Matrix2d D = Eigen::Matrix2d::Identity();
	D.bottomRightCorner<1, 1>()(0, 0) = d;
	Eigen::Matrix2d R = V * D * U.transpose();

	Eigen::Vector2d translation = mapCentroid - R * sampleCentroid;

	transform_t trf = transform_t::Identity();
	trf.topLeftCorner<2, 2>() = R;
	trf.topRightCorner<2, 1>() = translation;
	return trf;
}

TrICP::TrICP(int maxIter, double relErrChangeThresh,
			 std::function<point_t(const point_t&)> getClosest)
	: maxIter(maxIter), relErrChangeThresh(relErrChangeThresh),
	  getClosest(std::move(getClosest)) {
}

transform_t TrICP::correct(const points_t& sample, double overlap) {
	if (sample.empty() || overlap == 0) {
		return transform_t::Identity();
	}
	int i = 0;
	double mse = 1e9;
	double oldMSE;
	points_t points = sample;
	transform_t trf = transform_t::Identity();
	int N = static_cast<int>(overlap * sample.size());
	do {
		i++;
		oldMSE = mse;
		transform_t t = iterate(points, N, mse);
		trf = t * trf;
	} while (!isDone(i, mse, oldMSE));

	return trf;
}

bool TrICP::isDone(int numIter, double mse, double oldMSE) const {
	if (mse <= 1e-9) {
		return true;
	}
	double relErrChange = fabs(mse - oldMSE) / mse;
	return numIter >= maxIter || relErrChange <= relErrChangeThresh;
}

transform_t TrICP::iterate(points_t& sample, int N, double& mse) const {
	PointPair pairs[sample.size()];
	for (size_t i = 0; i < sample.size(); i++) {
		const point_t& point = sample[i];
		point_t closestPoint = getClosest(point);
		double dist = (point - closestPoint).norm();
		PointPair pair{closestPoint, point, dist};
		pairs[i] = pair;
	}

	std::vector<PointPair> closestPairs = getMinN(pairs, sample.size(), N);

	double newS = 0;
	for (const PointPair& pair : closestPairs) {
		newS += pair.dist * pair.dist;
	}
	mse = newS / N;

	transform_t trf = computeTransformation(closestPairs);

	for (point_t& point : sample) {
		point = trf * point;
	}
	return trf;
}


#ifndef EXPSUM_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP
#define EXPSUM_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP

#include <armadillo>

namespace expsum
{
///
/// Partial Lanczos bidiagonalization with full reorthogonalization
///
/// @param matvec  compute `y = a * A) * x + b * y` with matrix-free form
/// @param matvec_trans compute `y = a * A.t() * x + b * y` with matrix-free
///   form
/// @param[out] alpha  real vector of size `n` that stores diagonal part of B.
/// @param[out] beta   real vector of size `n - 1` that stores superdiagonal
///   part of B.
/// @param[out] P  on exit matrix P
/// @param[out] Q  on exit matrix Q
/// @param[out] rank  on exit rank of matrix A
/// @param[inout] tol_error on entry tolerance of residuals, and on exit an
///   esitmation of residual.
/// @param[inout] work workspace
///
template <typename MatVec, typename MatVecTrans, typename T>
void partial_lanczos_bidiagonalization(
    MatVec matvec, MatVecTrans matvec_trans,
    arma::Col<typename arma::Col<T>::pod_type>& alpha,
    arma::Col<typename arma::Col<T>::pod_type>& beta, arma::Mat<T>& P,
    arma::Mat<T>& Q, arma::uword& rank,
    typename arma::Col<T>::pod_type& tol_error, arma::Col<T>& work)
{
    using value_type = T;
    using real_type  = typename arma::Col<T>::pod_type;

    // constexpr static const value_type zero = value_type();
    // constexpr static const value_type one  = value_type(1);

    arma::uword m        = P.n_rows;
    arma::uword n        = Q.n_rows;
    arma::uword k        = P.n_cols;
    arma::uword max_rank = std::min({m, n, k});

    assert(Q.n_cols == k);
    assert(work.size() >= max_rank);
    assert(real_type() < tol_error && tol_error < real_type(1));

    auto q0 = Q.col(0);
    q0.randn();
    q0 /= arma::norm(q0);
    auto p0 = P.col(0);
    // p0 <-- A * q0
    matvec(q0, value_type(), p0);
    auto a1 = arma::norm(p0);
    if (a1 > real_type())
    {
        p0 *= real_type(1) / a1;
    }
    alpha(0) = a1;

    const real_type tol2 = tol_error * tol_error;
    // Estimation of the Frobenius norm of A
    real_type fnormA = a1 * a1;
    // Estimation of relative error
    real_type error = real_type();

    rank = 0;

    while (++rank < max_rank)
    {
        auto p1 = P.col(rank - 1);
        auto p2 = P.col(rank);
        auto q1 = Q.col(rank - 1);
        auto q2 = Q.col(rank);
        //
        // --- Recursion for right Lanczos vector
        //
        // q2 <-- A.t() * p1 - a1 * q1
        matvec_trans(p1, value_type(), q2);
        q2 -= a1 * q1;
        // Reorthogonalization
        auto tmp   = work.head(rank);
        auto viewQ = Q.head_cols(rank);
        tmp        = viewQ.t() * q2;
        q2 -= viewQ * tmp;

        auto b1 = arma::norm(q2);
        if (b1 > real_type())
        {
            q2 *= real_type(1) / b1;
        }
        beta(rank - 1) = b1;
        //
        // --- Recursion for left Lanczos vector
        //
        // p2 <-- A * p2 - b1 * p1
        matvec(q2, value_type(), p2);
        p2 -= b1 * p1;
        // Reorthogonalization
        auto viewP = P.head_cols(rank);
        tmp        = viewP.t() * p2;
        p2 -= viewP * tmp;

        const auto a2 = arma::norm(p2);
        if (a2 > real_type())
        {
            p2 *= real_type(1) / a2;
        }
        alpha(rank) = a2;

        const auto t = a2 * a2 + b1 * b1;
        //
        // Estimation of the Frobenius norm of A
        //
        // \|A\|_{F}^{2}= \sum_{K=1}^{rank(A)-1}
        //              (\alpha_{K}^{2} + \beta_{K}^{2}) + \alpha_{rank(A)}}
        //
        fnormA += t;
        error = t;
        if (t <= tol2 * fnormA)
        {
            // Converged
            break;
        }

        a1 = a2;
    }

    tol_error = sqrt(error);
    return;
}

} // namespace: expsum

#endif /* NUMERIC_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP */


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <complex>
#include <cmath>

using Eigen::MatrixXcd;
using Eigen::VectorXd;
using Eigen::VectorXcd;

#define AU_L 5.2917721067e-11 // m
#define AU_T 2.41888432651e-17 // s
#define AU_E 4.35974465e-18 // J
#define EV 1.6021766208e-19 // J
#define AU2ANG (AU_L / 1e-10)
#define AU2EV (AU_E / EV)
#define PI 3.141592653589793238462643383279502884L

double simps(VectorXcd f, int size, double dx)
{
    double so = 0.0, se = 0.0;
    for (int i = 1; i < size - 2; i++)
    {
        if(i % 2 == 1)
        {
            so = so + ((double)real(f[i]));
        }
        else
        {
            se = se + ((double)real(f[i]));
        }
    }
    return dx / 3 * (((double)real(f[0])) + ((double)real(f[size-1])) + 4 * so + 2 * se);
}

int main()
{
    // constantes do problema
    const double E0 = 150.0; // eV
    const double delta_x = 1.0; // angstron
    const double x0 = -20.0; // angstron

    // otimizando
    double L = 100.0; // angstron
    int N = 256;
    double dt = 1e-19; // s

    double L_au = L / AU2ANG;
    double dt_au = dt / AU_T;
    double E0_au = E0 / AU2EV;
    double delta_x_au = delta_x / AU2ANG;
    double x0_au = x0 / AU2ANG;
    double k0_au = sqrt(2.0 * E0_au);

    double dx = L / ((double)(N-1));
    double dx_au = L_au / ((double)(N-1));
    VectorXd x_au(N), x_aux(N);
    for (int i = 0; i < N; i++)
    {
        x_au[i] = -L_au / 2.0 + dx_au * ((double)i);
    }

    // crank-nicolson
    MatrixXcd B(N,N);
    MatrixXcd C(N,N);
    MatrixXcd D(N,N);

    std::complex<double> alpha = - dt_au * (1i / (2.0 * dx_au * dx_au)) / 2.0;
    std::complex<double> beta = 1.0 - dt_au * (- 1i / (dx_au * dx_au)) / 2.0;
    std::complex<double> gamma = 1.0 + dt_au * (- 1i / (dx_au * dx_au)) / 2.0;
    for (int i = 0; i < N; i++)
    {
        if (i > 0)
        {
            B(i,i-1) = alpha;
            C(i,i-1) = -alpha;
        }
        if (i < N - 1) {

            B(i,i+1) = alpha;
            C(i,i+1) = -alpha;
        }
        B(i,i) = beta;
        C(i,i) = gamma;
    }
    D = B.inverse() * C;

    // pacote de onda
    double d2 = pow(delta_x_au, 2);
    double PN = 1.0 / pow(2.0 * PI * d2, 1.0 / 4.0);
    VectorXcd psi(N), psi_aux(N), psi_s(N);
    for (int i = 0; i < N; i++)
    {
        psi[i] = 1i * k0_au * x_au[i] - pow(x_au[i] - x0_au, 2) / (4.0 * d2);
        psi[i] = PN * exp(psi[i]);
    }
    psi_s = psi.conjugate();

    double A0 = simps(psi_s.cwiseProduct(psi), N, dx_au);
    double A = 0.0;
    double xm = x0_au, xm2, xm3;
    double var_norma = 0.0;
    int contador = 0;

    while (xm < -x0_au)
    {
        contador++;
        psi = D * psi;
        psi_s = psi.conjugate();
        A = simps(psi_s.cwiseProduct(psi), N, dx_au);
        xm = simps(psi_s.cwiseProduct(x_au.cwiseProduct(psi)), N, dx_au) / A;
        //std::cout << "x0=" << x0_au * AU2ANG << " & A/A0=" << (100.0 * A / A0) << " & X=" << xm * AU2ANG << " & C=" << contador << "\n";
    }

    var_norma = 100.0 * A / A0;
    x_aux = x_au.cwiseProduct(x_au);
    xm2 = simps(psi_s.cwiseProduct(x_aux.cwiseProduct(psi)), N, dx_au) / A;
    x_aux = x_au.cwiseProduct(x_aux);
    xm3 = simps(psi_s.cwiseProduct(x_aux.cwiseProduct(psi)), N, dx_au) / A;
    double desvpad = sqrt(abs(xm2 - pow(xm, 2)));
    double skewness = (xm3 - 3.0 * xm * pow(desvpad, 2) - pow(xm, 3)) / pow(desvpad, 3);

    std::cout << "L=" << L << " & N=" << N << " & dt=" << dt << " & A/A0=" << var_norma << " & X=" << xm * AU2ANG << " & S=" << desvpad * AU2ANG << " & G=" << skewness << " & C=" << contador << " & T=" << (((double)contador) * dt) << "\n";
}


#include "benchmark_functions.hpp"

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

namespace optimization
{

namespace
{
template <typename Type>
std::vector<Type> list_to_vector(const boost::python::list &l)
{
    std::vector<Type> out(len(l));
    for (auto i = 0u; i < len(l); ++i)
        out.emplace_back(boost::python::extract<Type>(l[i]));

    return out;
}
}

double bent_cigar(const std::vector<double> &x)
{
    return std::pow(x[0], 2) +
           std::pow(10, 6) * std::accumulate(std::next(x.begin()), x.end(), 0., [](double acc, double el) {
               return acc + std::pow(el, 2);
           });
}

double bent_cigar_pywrapper(const boost::python::list &x)
{
    return bent_cigar(list_to_vector<double>(x));
}

double rosenbrock(const std::vector<double> &x)
{
    double result = 0;

    for (auto i = 0u; i < x.size() - 1; ++i)
        result += 100 * std::pow(std::pow(x[i], 2) - std::pow(x[i + 1], 2), 2) + std::pow(x[i] - 1, 2);

    return result;
}

double rosenbrock_pywrapper(const boost::python::list &x)
{
    return rosenbrock(list_to_vector<double>(x));
}

double rastrigin(const std::vector<double> &x)
{
    return std::accumulate(x.begin(), x.end(), 0., [](double acc, double el) {
        return acc + std::pow(el, 2) - 10. * std::cos(2 * std::acos(-1) * el) + 10.;
    });
}

double rastrigin_pywrapper(const boost::python::list &x)
{
    return rastrigin(list_to_vector<double>(x));
}

double zakharov(const std::vector<double> &x)
{
    const double by_half = std::accumulate(x.begin(), x.end(), 0., [](double acc, double el) {
        return el * 0.5;
    });

    return std::accumulate(x.begin(), x.end(), 0., [](double acc, double el) {
               return acc + std::pow(el, 2);
           }) +
           std::pow(by_half, 2.) + std::pow(by_half, 4.);
}

double zakharov_pywrapper(const boost::python::list &x)
{
    return zakharov(list_to_vector<double>(x));
}

double objective_function(function f, const std::vector<double> &x)
{
    switch (f)
    {
    case function::bent_cigar:
        return bent_cigar(x);
    case function::rosenbrock:
        return rosenbrock(x);
    case function::rastrigin:
        return rastrigin(x);
    case function::zakharov:
        return zakharov(x);
    }

    throw std::runtime_error("Wrong enum value for function");
}

} // optimization


// chebyshev.cpp: evaulation of Chebyshev polynomials for filter design
//
// Copyright (C) 2017-2021 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <hprblas>

// define a true 256-bit IEEE floating point type
constexpr size_t bits_in_octand = 113 + 128;
using cpp_bin_float_octand = boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<bits_in_octand, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -16382, 16383>, boost::multiprecision::expression_template_option::et_off>;
// define the floating point types (single, double, quad, octand)
using sp = boost::multiprecision::cpp_bin_float_single;
using dp = boost::multiprecision::cpp_bin_float_double;
using qp = boost::multiprecision::cpp_bin_float_quad;
using op = cpp_bin_float_octand;

// generate the Chebyshev nodes in the interval (-1, 1)
template<typename Vector>
void chebyshev_nodes(Vector& args, Vector& v) {
	typedef typename Vector::value_type value_type;
	size_t nrOfNodes = size(args);
	if (size(v) != size(args)) {
		std::cerr << "chebyshev_nodes: vectors must be the same size\n";
		return;
	}
	value_type pi = value_type(3.14159265358979323846);  // TODO: this is limited to native type accuracy
	for (size_t k = 1; k <= nrOfNodes; ++k) {
		value_type numerator = (2 * k - 1) * pi;
		value_type denominator = 2 * nrOfNodes;
		value_type arg = numerator / denominator; // better would be to use cospi arguments, so we side step radian round-off
		args[k] =arg;
		v[k] = cos(arg);
	}
}

template<typename Vector>
void dumpPair(const Vector& args, const Vector& nodes) {
	for (size_t i = 0; i < size(args) && i < size(nodes); ++i) {
		std::cout << args[i] << " : " << nodes[i] << std::endl;
	}
}

template<typename Scalar>
std::ostream& operator<<(std::ostream& ostr, const std::vector<Scalar>& vec) {
	ostr << "[ ";
	for (auto v : vec) {
		ostr << v << " ";
	}
	ostr << "]";
	return ostr;
}

int main(int argc, char** argv)
try {
	using namespace sw::universal;

	using Real = double; // qp;
	mtl::vec::dense_vector<Real> args(10), nodes(10);
	chebyshev_nodes(args, nodes);
	std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
	dumpPair(args, nodes);

	mtl::vec::cos(args);

	std::cout << nodes << std::endl;

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}

/*
 BOOST implementation of operator<<() for cpp_bin_float backend

 class number {
 ...
   //
   // String conversion functions:
   //
   std::string str(std::streamsize digits = 0, std::ios_base::fmtflags f = std::ios_base::fmtflags(0))const
   {
	  return m_backend.str(digits, f);
   }
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
   ar & m_backend;
   }
...
}

template <class Backend, expression_template_option ExpressionTemplates>
inline std::ostream& operator << (std::ostream& os, const number<Backend, ExpressionTemplates>& r)
{
	std::streamsize d = os.precision();
	std::string s = r.str(d, os.flags());
	std::streamsize ss = os.width();
	if (ss > static_cast<std::streamsize>(s.size()))
	{
		char fill = os.fill();
		if ((os.flags() & std::ios_base::left) == std::ios_base::left)
			s.append(static_cast<std::string::size_type>(ss - s.size()), fill);
		else
			s.insert(static_cast<std::string::size_type>(0), static_cast<std::string::size_type>(ss - s.size()), fill);
	}
	return os << s;
}

 template <unsigned Digits, digit_base_type DigitBase, class Allocator, class Exponent, Exponent MinE, Exponent MaxE>
std::string cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::str(std::streamsize dig, std::ios_base::fmtflags f) const
{
   if(dig == 0)
	  dig = std::numeric_limits<number<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE> > >::max_digits10;

   bool scientific = (f & std::ios_base::scientific) == std::ios_base::scientific;
   bool fixed = !scientific && (f & std::ios_base::fixed);

   std::string s;

   if(exponent() <= cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::max_exponent)
   {
	  // How far to left-shift in order to demormalise the mantissa:
	  boost::intmax_t shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1;
	  boost::intmax_t digits_wanted = static_cast<int>(dig);
	  boost::intmax_t base10_exp = exponent() >= 0 ? static_cast<boost::intmax_t>(std::floor(0.30103 * exponent())) : static_cast<boost::intmax_t>(std::ceil(0.30103 * exponent()));
	  //
	  // For fixed formatting we want /dig/ digits after the decimal point,
	  // so if the exponent is zero, allowing for the one digit before the
	  // decimal point, we want 1 + dig digits etc.
	  //
	  if(fixed)
		 digits_wanted += 1 + base10_exp;
	  if(scientific)
		 digits_wanted += 1;
	  if(digits_wanted < -1)
	  {
		 // Fixed precision, no significant digits, and nothing to round!
		 s = "0";
		 if(sign())
			s.insert(static_cast<std::string::size_type>(0), 1, '-');
		 boost::multiprecision::detail::format_float_string(s, base10_exp, dig, f, true);
		 return s;
	  }
	  //
	  // power10 is the base10 exponent we need to multiply/divide by in order
	  // to convert our denormalised number to an integer with the right number of digits:
	  //
	  boost::intmax_t power10 = digits_wanted - base10_exp - 1;
	  //
	  // If we calculate 5^power10 rather than 10^power10 we need to move
	  // 2^power10 into /shift/
	  //
	  shift -= power10;
	  cpp_int i;
	  int roundup = 0; // 0=no rounding, 1=tie, 2=up
	  static const unsigned limb_bits = sizeof(limb_type) * CHAR_BIT;
	  //
	  // Set our working precision - this is heuristic based, we want
	  // a value as small as possible > cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count to avoid large computations
	  // and excessive memory usage, but we also want to avoid having to
	  // up the computation and start again at a higher precision.
	  // So we round cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count up to the nearest whole number of limbs, and add
	  // one limb for good measure.  This works very well for small exponents,
	  // but for larger exponents we add a few extra limbs to max_bits:
	  //
#ifdef BOOST_MP_STRESS_IO
	  boost::intmax_t max_bits = cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count + 32;
#else
	  boost::intmax_t max_bits = cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count + ((cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count % limb_bits) ? (limb_bits - cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count % limb_bits) : 0) + limb_bits;
	  if(power10)
		 max_bits += (msb(boost::multiprecision::detail::abs(power10)) / 8) * limb_bits;
#endif
	  do
	  {
		 boost::int64_t error = 0;
		 boost::intmax_t calc_exp = 0;
		 //
		 // Our integer result is: bits() * 2^-shift * 5^power10
		 //
		 i = bits();
		 if(shift < 0)
		 {
			if(power10 >= 0)
			{
			   // We go straight to the answer with all integer arithmetic,
			   // the result is always exact and never needs rounding:
			   BOOST_ASSERT(power10 <= (boost::intmax_t)INT_MAX);
			   i <<= -shift;
			   if(power10)
				  i *= pow(cpp_int(5), static_cast<unsigned>(power10));
			}
			else if(power10 < 0)
			{
			   cpp_int d;
			   calc_exp = boost::multiprecision::cpp_bf_io_detail::restricted_pow(d, cpp_int(5), -power10, max_bits, error);
			   shift += calc_exp;
			   BOOST_ASSERT(shift < 0); // Must still be true!
			   i <<= -shift;
			   cpp_int r;
			   divide_qr(i, d, i, r);
			   roundup = boost::multiprecision::cpp_bf_io_detail::get_round_mode(r, d, error, i);
			   if(roundup < 0)
			   {
#ifdef BOOST_MP_STRESS_IO
				  max_bits += 32;
#else
				  max_bits *= 2;
#endif
				  shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
				  continue;
			   }
			}
		 }
		 else
		 {
			//
			// Our integer is bits() * 2^-shift * 10^power10
			//
			if(power10 > 0)
			{
			   if(power10)
			   {
				  cpp_int t;
				  calc_exp = boost::multiprecision::cpp_bf_io_detail::restricted_pow(t, cpp_int(5), power10, max_bits, error);
				  calc_exp += boost::multiprecision::cpp_bf_io_detail::restricted_multiply(i, i, t, max_bits, error);
				  shift -= calc_exp;
			   }
			   if((shift < 0) || ((shift == 0) && error))
			   {
				  // We only get here if we were asked for a crazy number of decimal digits -
				  // more than are present in a 2^max_bits number.
#ifdef BOOST_MP_STRESS_IO
				  max_bits += 32;
#else
				  max_bits *= 2;
#endif
				  shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
				  continue;
			   }
			   if(shift)
			   {
				  roundup = boost::multiprecision::cpp_bf_io_detail::get_round_mode(i, shift - 1, error);
				  if(roundup < 0)
				  {
#ifdef BOOST_MP_STRESS_IO
					 max_bits += 32;
#else
					 max_bits *= 2;
#endif
					 shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
					 continue;
				  }
				  i >>= shift;
			   }
			}
			else
			{
			   // We're right shifting, *and* dividing by 5^-power10,
			   // so 5^-power10 can never be that large or we'd simply
			   // get zero as a result, and that case is already handled above:
			   cpp_int r;
			   BOOST_ASSERT(-power10 < INT_MAX);
			   cpp_int d = pow(cpp_int(5), static_cast<unsigned>(-power10));
			   d <<= shift;
			   divide_qr(i, d, i, r);
			   r <<= 1;
			   int c = r.compare(d);
			   roundup = c < 0 ? 0 : c == 0 ? 1 : 2;
			}
		 }
		 s = i.str(0, std::ios_base::fmtflags(0));
		 //
		 // Check if we got the right number of digits, this
		 // is really a test of whether we calculated the
		 // decimal exponent correctly:
		 //
		 boost::intmax_t digits_got = i ? static_cast<boost::intmax_t>(s.size()) : 0;
		 if(digits_got != digits_wanted)
		 {
			base10_exp += digits_got - digits_wanted;
			if(fixed)
			   digits_wanted = digits_got;  // strange but true.
			power10 = digits_wanted - base10_exp - 1;
			shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
			if(fixed)
			   break;
			roundup = 0;
		 }
		 else
			break;
	  }
	  while(true);
	  //
	  // Check whether we need to round up: note that we could equally round up
	  // the integer /i/ above, but since we need to perform the rounding *after*
	  // the conversion to a string and the digit count check, we might as well
	  // do it here:
	  //
	  if((roundup == 2) || ((roundup == 1) && ((s[s.size() - 1] - '0') & 1)))
	  {
		 boost::multiprecision::detail::round_string_up_at(s, static_cast<int>(s.size() - 1), base10_exp);
	  }

	  if(sign())
		 s.insert(static_cast<std::string::size_type>(0), 1, '-');

	  boost::multiprecision::detail::format_float_string(s, base10_exp, dig, f, false);
   }
   else
   {
	  switch(exponent())
	  {
	  case exponent_zero:
		 s = sign() ? "-0" : f & std::ios_base::showpos ? "+0" : "0";
		 boost::multiprecision::detail::format_float_string(s, 0, dig, f, true);
		 break;
	  case exponent_nan:
		 s = "nan";
		 break;
	  case exponent_infinity:
		 s = sign() ? "-inf" : f & std::ios_base::showpos ? "+inf" : "inf";
		 break;
	  }
   }
   return s;
}
 */


#pragma once
#include <array>
#include <cassert>
#include <type_traits>

#include <Eigen/Core>

#include "control_points_container.hpp"
#include "fixed_size_container_type_trait.hpp"
#include "fixed_size_container_type_trait_eigen.hpp"
#include "multi_array.hpp"
#include "value_type_trait.hpp"
#include "internal/base_matrix.hpp"
#include "internal/gtest_friend.hpp"
#include "internal/no_discard.hpp"
#include "internal/uniform_bspline_eval.hpp"

namespace ubs {

/**
 * @brief A uniform B-spline.
 *
 * An implementation of a uniform B-spline from @f$ \mathbb{R}^n \rightarrow \mathbb{R}^m @f$. Uniform means, that the
 * knot vector of the B-spline is equally distributed. Using such a knot vector makes the computation much more
 * efficient, as the basis can be precomputed.
 *
 * For an introduction to B-splines see <https://en.wikipedia.org/wiki/B-spline>.
 *
 * The input and output dimensions of the spline is defined by the input and output type.
 *
 * @tparam ValueType_ The value type.
 * @tparam Degree_ The spline degree.
 * @tparam InputType_ The input type. A FixedSizeContainerTypeTrait must be available for that type.
 * @tparam OutputType_ The output type. A FixedSizeContainerTypeTrait must be available for that type.
 * @tparam ControlPointsType_ The control points type. A ControlPointsTrait must be available for that
 *                            type.
 */
template <typename ValueType_,
          int Degree_,
          typename InputType_,
          typename OutputType_,
          typename ControlPointsType_ = ubs::MultiArray<OutputType_,
                                                        FixedSizeContainerTypeTrait<InputType_>::Size,
                                                        typename FixedSizeContainerTypeTrait<OutputType_>::Allocator>>
class UniformBSpline {
    // Friends for classes, which need to be partial specialized. (These classes cannot be functions as in class
    // partial specialization is not allowed.)
    template <typename Spline_, int InputDims_, int CurDim_>
    friend struct internal::EvaluateBSpline;
    template <int TotalDerivative_, int PartialDerivativeIdx_, int InputDims_, int CurDim_>
    friend class internal::EvaluateBSplineSmoothness;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /** @brief The spline degree. */
    static constexpr int Degree = Degree_;
    /** @brief The spline order. */
    static constexpr int Order = Degree_ + 1;
    /** @brief The number of input dimensions. */
    static constexpr int InputDims = FixedSizeContainerTypeTrait<InputType_>::Size;
    /** @brief The number of output dimensions. */
    static constexpr int OutputDims = FixedSizeContainerTypeTrait<OutputType_>::Size;

    static_assert(InputDims > 0, "The number of input dimensions must be positive.");
    static_assert(OutputDims > 0, "The number of output dimensions must be positive.");

    // If one would like to use a higher degree spline, the basis matrices needs to be generated. The code generation
    // is written in a Mathematica script located at scripts/generate_bspline_basis_cpp_files.
    static_assert(Degree >= 1 && Degree <= 5, "Unsupported degree specified.");

    /** @brief The value type. */
    using ValueType = ValueType_;

    /** @brief The input type. */
    using InputType = InputType_;
    /** @brief The output type. */
    using OutputType = OutputType_;

    /** @brief The B-spline basis type. */
    using BasisType = Eigen::Matrix<ValueType, Order, 1>;
    /** @brief The control points type. */
    using ControlPointsType = ControlPointsType_;
    /** @brief The control points container type. */
    using ControlPointsContainerType =
        ControlPointsContainer<ValueType, Degree, InputType, OutputType, ControlPointsType>;

    /**
     * @brief Default constructor.
     *
     * Creates a uniform B-spline for which all control points are initialized with zero.
     * The number of control points in each dimension is the lowest number possible, which is Order.
     * The lower bound is set to zero and the upper bound is set to one in each dimension.
     */
    UniformBSpline() = default;

    /**
     * @brief Constructs a uniform B-spline using the specified control points.
     *
     * The number of control points for each dimension must be at least the order of the spline.
     * The lower bound is set to zero and the upper bound is set to one in each dimension.
     *
     * @param[in] controlPoints The control points.
     */
    explicit UniformBSpline(const ControlPointsType& controlPoints) : controlPoints_(controlPoints) {
    }

    /**
     * @brief Constructs a uniform B-spline using the specified lower bound and upper bound.
     *
     * All the control points are zero initialized. The number of control points in each dimension is the lowest
     * number possible, which is Order.
     *
     * @param[in] lowerBound The lower bound.
     * @param[in] upperBound The upper bound.
     */
    UniformBSpline(const InputType& lowerBound, const InputType& upperBound) : controlPoints_(lowerBound, upperBound) {
    }

    /**
     * @brief Constructs a uniform B-spline using the specified control points, lower bound and upper bound.
     *
     * @note The number of control points for each dimension must be at least the order of the spline.
     * @param[in] controlPoints The control points.
     * @param[in] lowerBound The lower bound.
     * @param[in] upperBound The upper bound.
     */
    UniformBSpline(const InputType& lowerBound, const InputType& upperBound, const ControlPointsType& controlPoints)
            : controlPoints_(lowerBound, upperBound, controlPoints) {
    }

    /**
     * @brief Construct a new Uniform B Spline object using a control points container.
     *
     * @param[in] controlPointsContainer The control points container.
     */
    explicit UniformBSpline(const ControlPointsContainerType& controlPointsContainer)
            : controlPoints_(controlPointsContainer) {
    }

    /** @copydoc UniformBSpline(const ControlPointsContainerType& controlPointsContainer) */
    explicit UniformBSpline(ControlPointsContainerType&& controlPointsContainer)
            : controlPoints_(std::move(controlPointsContainer)) {
    }

    /**
     * @brief Set new control points.
     * @param[in] controlPoints The new control points.
     */
    void setControlPoints(const ControlPointsType& controlPoints) {
        controlPoints_.set(controlPoints);
    }

    /** @copydoc setControlPoints() */
    void setControlPoints(ControlPointsType&& controlPoints) {
        controlPoints_.set(std::move(controlPoints));
    }

    /**
     * @brief Set new bounds.
     * @note lowerBound < upperBound for each dimensions.
     * @param[in] lowerBound The lower range of the input type.
     * @param[in] upperBound The upper range of the input type.
     */
    void setBounds(const InputType& lowerBound, const InputType& upperBound) {
        controlPoints_.setBounds(lowerBound, upperBound);
    }

    /**
     * @brief Checks, whether the specified position is between lower and upper bound.
     * @param[in] pos The position.
     * @return True, if this point can be used to evaluate the B-spline, otherwise false.
     */
    UBS_NO_DISCARD bool inRange(const InputType& pos) const {
        using ContainerTrait = FixedSizeContainerTypeTrait<InputType>;
        for (int dim = 0; dim < InputDims; ++dim) {
            const auto& val = ContainerTrait::get(pos, dim);
            if (val < getLowerBound(dim) || val > getUpperBound(dim)) {
                return false;
            }
        }

        return true;
    }

    /**
     * @return The lower bound in each dimension.
     */
    UBS_NO_DISCARD const InputType& getLowerBound() const {
        return controlPoints_.getLowerBound();
    }

    /**
     * @return The upper bound in each dimension.
     */
    UBS_NO_DISCARD const InputType& getUpperBound() const {
        return controlPoints_.getUpperBound();
    }

    /**
     * @brief Return the lower bound for the specified dimension.
     * @param[in] dim The dimension.
     * @return The lower bound.
     */
    UBS_NO_DISCARD ValueType getLowerBound(int dim) const {
        return controlPoints_.getLowerBound(dim);
    }

    /**
     * @brief Return the upper bound for the specified dimension.
     * @param[in] dim The dimension.
     * @return The upper bound.
     */
    UBS_NO_DISCARD ValueType getUpperBound(int dim) const {
        return controlPoints_.getUpperBound(dim);
    }

    /**
     * @brief Returns the number of control points of the specified dimension.
     * @param[in] dim The dimension.
     * @return The number of control points.
     */
    UBS_NO_DISCARD int getNumControlPoints(int dim) const {
        return int(controlPoints_.getSize(dim));
    }

    /**
     * @return The constant control points.
     */
    UBS_NO_DISCARD const ControlPointsType& getControlPoints() const {
        return controlPoints_.get();
    }

    /**
     * @brief Returns the control points.
     * @return The control points.
     */
    UBS_NO_DISCARD const ControlPointsContainerType& getControlPointsContainer() const {
        return controlPoints_;
    }

    /** @copydoc getControlPointsContainer() const */
    UBS_NO_DISCARD ControlPointsContainerType& getControlPointsContainer() {
        return controlPoints_;
    }

    /**
     * @brief Returns the scale of the B-spline.
     *
     * The scales are depending on the number of control points and the lower and upper bounds.
     *
     * @param[in] dim The dimension.
     * @return The scale in the specified dimension.
     */
    UBS_NO_DISCARD ValueType getScale(int dim) const {
        return controlPoints_.getScale(dim);
    }

    /**
     * @brief Set extrapolation.
     *
     * If set to true, the spline can be queried outside of the bounds. As an extrapolation the closest spline segment
     * is used.
     *
     * @param[in] v True, if extrapolating should be enabled, otherwise false.
     */
    void setExtrapolate(bool v) {
        extrapolate_ = v;
    }

    /**
     * @sa setExtrapolate(bool v)
     * @return True, if extrapolation is enabled, otherwise false.
     */
    UBS_NO_DISCARD bool isExtrapolating() const {
        return extrapolate_;
    }

    /**
     * @brief Determine the span and value in the specified dimension at the specified value.
     *
     * The span is the start position, which needs to be used together with the basis to evaluate the spline at val.
     * The span value is the value which is needed to get the basis function at the correct position. This means,
     * one can calculate the spline value as follows (1D -> 1D):
     *
     * @code
     * double pos = 0.1;
     * const std::pair<int, double> spanVal = getSpanAndValue(0, pos);
     * auto basis = basisFunctions(spanVal.second);
     *
     * double ret = 0.0;
     * int idx = 0;
     * for (int i = spanVal.first; i < spanVal.first + Order; ++i, ++idx) {
     *      ret += basis[idx] * controlPoint[i];
     * }
     * @endcode
     *
     * @param[in] dim The dimension.
     * @param[in] val The position (0 <= x <= 1).
     * @return A pair, where first is the span index and second is the span value.
     */
    UBS_NO_DISCARD std::pair<int, ValueType> getSpanIndexAndValue(int dim, ValueType val) const {
        assert(controlPoints_.getScale(dim) > ValueType(0.0));

        if (!extrapolate_) {
            assert(val >= getLowerBound(dim) - ValueType(1e-5) && val <= getUpperBound(dim) + ValueType(1e-5));
        }

        const ValueType baseX = (val - controlPoints_.getLowerBound(dim)) * controlPoints_.getScale(dim);

        // Use trait to convert to int. For 'normal' types this is just a static cast, but e.g. for a ceres::Jet type
        // the value part is used and cast to int.
        const int xii = ValueTypeTrait<ValueType>::toInt(baseX);
        const auto xi = std::max(0, std::min(xii, getNumControlPoints(dim) - Order));
        const ValueType xv = baseX - ValueType(xi);

        return std::make_pair(xi, xv);
    }

    /**
     * @brief Return the start index and values.
     *
     * These information can be used to evaluate the B-spline and its derivatives.
     * The returned data is only valid as long as the shape of the control points and the lower and upper bound is not
     * changed.
     *
     * @sa getSpanIndexAndValue(int dim, ValueType val) const
     * @param[in] pos The position to evaluate the start index and values in each dimension.
     * @return std::pair<int, InputType>
     */
    UBS_NO_DISCARD std::pair<int, InputType> getStartIndexAndValues(const InputType& pos) const {
        std::pair<int, InputType> data{};

        const auto& strides = controlPoints_.getStrides();
        for (int dim = 0; dim < InputDims; ++dim) {
            const ValueType val = FixedSizeContainerTypeTrait<InputType>::get(pos, dim);
            const auto sv = getSpanIndexAndValue(dim, val);

            data.first += strides[dim] * sv.first;
            FixedSizeContainerTypeTrait<InputType>::get(data.second, dim) = sv.second;
        }

        return data;
    }

    /**
     * @sa getSpanIndexAndValue
     * @return The span index.
     */
    UBS_NO_DISCARD int getSpanIndex(int dim, ValueType point) const {
        return getSpanIndexAndValue(dim, point).first;
    }

    /**
     * @brief Evaluates the span indices.
     * @sa getSpanIndex(int dim, ValueType point) const
     * @param[in] pos The position to evaluate the spline indices.
     * @return The span indices in each dimension.
     */
    UBS_NO_DISCARD std::array<int, InputDims> getSpanIndices(const InputType& pos) const {
        std::array<int, InputDims> indices{};
        for (int dim = 0; dim < InputDims; ++dim) {
            indices[dim] = getSpanIndex(dim, FixedSizeContainerTypeTrait<InputType>::get(pos, dim));
        }
        return indices;
    }

    /**
     * @sa getSpanIndexAndValue
     * @return The span value.
     */
    UBS_NO_DISCARD ValueType getSpanValue(int dim, ValueType point) const {
        return getSpanIndexAndValue(dim, point).second;
    }

    /**
     * @brief Determine the basis at the span value.
     * @sa getSpanIndexAndValue
     * @param[in] spanValue The span value.
     * @return The basis.
     */
    UBS_NO_DISCARD BasisType basisFunctions(ValueType spanValue) const {
        BasisType basis{basis_.col(0)};
        for (int i = 1; i < Order; ++i) {
            basis = spanValue * basis + basis_.col(i);
        }
        return basis;
    }

    /**
     * @brief Determine the basis derivative at the span value.
     * @sa getSpanIndexAndValue
     * @param[in] dim The dimension.
     * @param[in] derivative The derivative.
     * @param[in] spanValue The span value.
     * @return The derivative basis.
     */
    UBS_NO_DISCARD BasisType basisFunctionDerivatives(int dim, int derivative, ValueType spanValue) const {
        if (derivative > Degree) {
            return BasisType::Zero();
        }
        if (derivative == 0) {
            return basisFunctions(spanValue);
        }

        using ValueTypeTrait = ValueTypeTrait<ValueType>;
        const ValueType scale{ValueTypeTrait::pow(controlPoints_.getScale(dim), derivative)};

        BasisType basis = basis_.col(0) * (derivativeFactors_(0, derivative - 1) * scale);
        for (int i = 1; i < Order - derivative; ++i) {
            basis = spanValue * basis + basis_.col(i) * (derivativeFactors_(i, derivative - 1) * scale);
        }
        return basis;
    }

    /**
     * @brief Evaluates the spline at the specified position.
     * @param[in] pos The position. Each value must be between lower bound and upper bound.
     * @return The spline value.
     */
    UBS_NO_DISCARD OutputType evaluate(const InputType& pos) const {
        return evaluate(pos, [this](int /*dim*/, ValueType pos) { return basisFunctions(pos); });
    }

    /**
     * @brief Evaluates the derivative of the spline at the specified position.
     * @param[in] pos The position. Each value must be between lower bound and upper bound.
     * @param[in] d The derivative in the first dimension.
     * @param[in] derivatives The derivative in the second, third, ... dimension.
     * @return The derivative value.
     */
    template <typename... Ts>
    UBS_NO_DISCARD OutputType derivative(const InputType& pos, int d, Ts... derivatives) const {
        static_assert(1 + sizeof...(Ts) == InputDims, "Invalid number of inputs specified.");
        const std::array<int, InputDims> derivs{d, derivatives...};
        return derivative(pos, derivs);
    }

    /**
     * @brief Evaluates the derivative of the spline at the specified position.
     * @param[in] pos The position. Each value must be between lower bound and upper bound.
     * @param[in] derivatives The partial derivatives.
     * @return The derivative value.
     */
    UBS_NO_DISCARD OutputType derivative(const InputType& pos, const std::array<int, InputDims>& derivatives) const {
        return evaluate(pos, [this, &derivatives](int dim, ValueType pos) {
            const int derivative = derivatives[dim];
            if (derivative == 0) {
                return basisFunctions(pos);
            }

            return basisFunctionDerivatives(dim, derivative, pos);
        });
    }

    /**
     * @brief Evaluates the smoothness value.
     *
     * The smoothness value is defined as:
     * @f[
     * s_i = \int_0^1 \lVert f_i^{(n)}(\mathbf{x}) \rVert^2 \mathbf{dx}
     * @f]
     *
     * @tparam TotalDerivative The total derivative.
     * @return The smoothness value in each dimension.
     */
    template <int TotalDerivative>
    UBS_NO_DISCARD OutputType smoothness() const {
        static_assert(TotalDerivative >= 0 && TotalDerivative <= 3, "Unsupported derivative specified.");
        constexpr int MaxTotal = internal::TotalDerivative<InputDims, TotalDerivative>::NumPartialDerivatives;

        OutputType res = FixedSizeContainerTypeTrait<OutputType>::zero();
        internal::ComputeSmoothness<InputDims, TotalDerivative, MaxTotal, 0>::apply(
            *this, controlPoints_.getStrides(), res);
        return res;
    }

    /**
     * @brief Evaluates the B-spline by calculating the basis and passes it to the evalFunction.
     *
     * This function iterates over the sums, which are needed to compute a B-spline. It calculates the basis value and
     * for each calculated basis value it calls the eval function. The signature must be void(int idx, ValueType
     * basisVal), where idx is the control point index and basisVal value which need to be multiplied with the control
     * point.
     *
     * This function is mainly used if one would like to get the basis values and control points indices and defer the
     * evaluation. This is usually the case during optimization.
     *
     * @param[in] pos The position. Each value must be between lower bound and upper bound.
     * @param[in] basisFunction The basis function.
     * @param[in] evalFunction The evaluation function.
     */
    template <typename BasisFunction, typename EvalFunction>
    void evaluate(const InputType& pos, BasisFunction basisFunction, EvalFunction evalFunction) const {
        const std::pair<int, InputType> startIndexValues = getStartIndexAndValues(pos);
        evaluate(startIndexValues.first, startIndexValues.second, basisFunction, evalFunction);
    }

    /**
     * @copybrief evaluate(const InputType& pos, BasisFunction basisFunction, EvalFunction evalFunction) const
     *
     * For more information see
     * evaluate(const InputType& pos, BasisFunction basisFunction, EvalFunction evalFunction) const.
     *
     * @param[in] startIdx The start index returned by getStartIndexAndValues().
     * @param[in] values The values returned by getStartIndexAndValues().
     * @param[in] basisFunction The basis function.
     * @param[in] evalFunction The evaluation function.
     */
    template <typename BasisFunction, typename EvalFunction>
    void evaluate(int startIdx, const InputType& values, BasisFunction basisFunction, EvalFunction evalFunction) const {
        const auto& strides = controlPoints_.getStrides();

        std::array<BasisType, InputDims> fullBasis{};
        for (int dim = 0; dim < InputDims; ++dim) {
            fullBasis[dim] = basisFunction(dim, FixedSizeContainerTypeTrait<InputType>::get(values, dim));
        }

        using SplineType = UniformBSpline<ValueType_, Degree_, InputType_, OutputType_, ControlPointsType>;
        internal::EvaluateBSpline<SplineType, InputDims, 0>::apply(
            fullBasis, strides, startIdx, ValueType(1.0), evalFunction);
    }

    /**
     * @brief Casts this spline to the spline specified in the template parameter SplineOut.
     *
     * The cast function copies the control points, the lower and upper bounds. The value type of the source and
     * destination types must be convertible.
     *
     * @tparam SplineOut The spline to which this spline is casted to.
     * @return The casted spline.
     */
    template <typename SplineOut,
              typename = std::enable_if_t<
                  std::is_same<UniformBSpline<ValueType_, Degree_, InputType_, OutputType_, ControlPointsType>,
                               SplineOut>::value>>
    UBS_NO_DISCARD const SplineOut& cast() {
        return *this;
    }

    /** \copydoc const SplineOut& cast() */
    template <typename SplineOut,
              typename = std::enable_if_t<
                  !std::is_same<UniformBSpline<ValueType_, Degree_, InputType_, OutputType_, ControlPointsType>,
                                SplineOut>::value>>
    UBS_NO_DISCARD SplineOut cast() const {
        using T = typename SplineOut::ValueType;
        using OptSpline = SplineOut;
        using OptInputType = typename OptSpline::InputType;
        using OptOutputType = typename OptSpline::OutputType;
        using OptControlPointsType = typename OptSpline::ControlPointsType;
        using OptControlPointsContainerType = typename OptSpline::ControlPointsContainerType;
        using OptInputContainerTrait = ubs::FixedSizeContainerTypeTrait<OptInputType>;
        using OptOutputContainerTrait = ubs::FixedSizeContainerTypeTrait<OptOutputType>;

        // Convert bounds.
        OptInputType lowerBound{};
        OptInputType upperBound{};
        for (int i = 0; i < OptInputContainerTrait::Size; ++i) {
            OptInputContainerTrait::get(lowerBound, i) = T(getLowerBound(i));
            OptInputContainerTrait::get(upperBound, i) = T(getUpperBound(i));
        }

        OptControlPointsType controlPoints{};
        ubs::ControlPointsTrait<OptControlPointsType>::resize(controlPoints, controlPoints_.getShape());
        OptControlPointsContainerType controlPointsContainer(lowerBound, upperBound, std::move(controlPoints));

        // Copy control points.
        controlPoints_.transform(controlPointsContainer, [](const auto& p) {
            OptOutputType v{};
            for (int i = 0; i < OptOutputContainerTrait::Size; ++i) {
                OptOutputContainerTrait::get(v, i) = T(ubs::FixedSizeContainerTypeTrait<OutputType>::get(p, i));
            }
            return v;
        });

        SplineOut splineOut(std::move(controlPointsContainer));
        splineOut.setExtrapolate(extrapolate_);
        return splineOut;
    }

private:
    // Used for unit test of derivative factors.
    FRIEND_TEST(UniformBSpline, DerivativeFactors);

    /**
     * @brief Evaluate the B-spline given a basis function.
     *
     * The evaluation of the function itself and the derivatives is similar, the basis function selects, which basis
     * is used.
     *
     * @param[in] pos The position, at which the B-spline will be evaluated.
     * @param[in] basisFunction The basis function. The signature must be @code BasisType(int dim, ValueType pos)
     * @endcode
     * @return The evaluated basis value.
     */
    template <typename BasisFunction>
    UBS_NO_DISCARD OutputType evaluate(const InputType& pos, BasisFunction basisFunction) const {
        OutputType res = FixedSizeContainerTypeTrait<OutputType>::zero();
        evaluate(pos, basisFunction, [this, &res](int idx, ValueType basisVal) {
            res += controlPoints_.at(idx) * basisVal;
        });
        return res;
    }

    /**
     * @brief Determines the derivative factors.
     *
     * The derivative factors are the factor from deriving the vector
     * @f[
     * \mathbf{x}(t) = \begin{pmatrix} \vdots  \\ t^2 \\ t \\ 1 \end{pmatrix}
     * @f]
     * n times.
     * As an example, the derivative factors for order 4 are:
     * @f[
     * \begin{blockarray}{cccc}
     * x^{(1)} & x^{(2)} & x^{(3)} & x^{(4)} \\
     * \begin{block}{(cccc)}
     * 4 & 12 & 24 & 24 \\
     * 3 &  6 &  6 &  0 \\
     * 2 &  2 &  0 &  0 \\
     * 1 &  0 &  0 &  0 \\
     * \end{block}
     * \end{blockarray}
     * @f]
     *
     * @return The derivative factors.
     */
    UBS_NO_DISCARD Eigen::Matrix<ValueType, Degree, Degree> getDerivativeFactors() const {
        Eigen::Matrix<int, Degree, Degree> derivativeFactors = Eigen::Matrix<int, Degree, Degree>::Zero();

        for (int i = 0; i < Degree; ++i) {
            derivativeFactors(i, 0) = Order - 1 - i;
        }

        for (int c = 1; c < Degree; ++c) {
            for (int r = 0; r < Degree - c; ++r) {
                derivativeFactors(r, c) = derivativeFactors(r, c - 1) * (Degree - r - c);
            }
        }

        return derivativeFactors.template cast<ValueType>();
    }

    /** @brief The control points. */
    ControlPointsContainerType controlPoints_;

    /** @brief The uniform B-spline basis. */
    Eigen::Matrix<ValueType, Order, Order> basis_{
        internal::UniformBSplineBasis<Order>::matrix().template cast<ValueType>()};
    /** @brief The derivative factors. */
    Eigen::Matrix<ValueType, Degree, Degree> derivativeFactors_{getDerivativeFactors()};
    /** @brief Flag, if spline extrapolation is enabled. If true, the spline can be evaluated outside of the bounds. */
    bool extrapolate_{false};
};
} // namespace ubs

#include "internal/uniform_bspline_impl.hpp"

namespace ubs {

/**
 * @brief Using declaration for a spline @f$ \mathbb{R} \rightarrow \mathbb{R} @f$.
 * @tparam ValueType_ The value type.
 * @tparam Degree_ The spline degree.
 */
template <typename ValueType, int Degree>
using UniformBSpline11 =
    UniformBSpline<ValueType,
                   Degree,
                   ValueType,
                   ValueType,
                   std::vector<ValueType, typename FixedSizeContainerTypeTrait<ValueType>::Allocator>>;

/**
 * @copybrief UniformBSpline11
 *
 * The value type is float.
 * @tparam Degree_ The spline degree.
 */
template <int Degree>
using UniformBSpline11f = UniformBSpline11<float, Degree>;

/**
 * @copybrief UniformBSpline11
 *
 * The value type is double.
 * @tparam Degree_ The spline degree.
 */
template <int Degree>
using UniformBSpline11d = UniformBSpline11<double, Degree>;

/**
 * @brief Using declaration for an Eigen spline @f$ \mathbb{R}^n \rightarrow \mathbb{R}^m @f$.
 * @tparam ValueType_ The value type.
 * @tparam Degree_ The spline degree.
 * @tparam InputDims_ The input dimensions.
 * @tparam OutputDims_ The output dimensions.
 */
template <typename ValueType, int Degree, int InputDims, int OutputDims>
using EigenUniformBSpline =
    UniformBSpline<ValueType, Degree, Eigen::Matrix<ValueType, InputDims, 1>, Eigen::Matrix<ValueType, OutputDims, 1>>;

} // namespace ubs


#ifndef _SPHERE_MESH_GEN_IMPL_H_
#define _SPHERE_MESH_GEN_IMPL_H_

#include "mtao/types.hpp"
#include <tuple>
#include <array>
#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <Eigen/Dense>

namespace mtao::geometry::mesh::shapes {
    namespace detail {
        template <typename Scalar_>
            class SphereMeshFactory{
                typedef std::array<unsigned int, 3> Face;
                typedef std::array<unsigned int, 2> Edge;
                public:
                typedef Scalar_ Scalar;
                typedef typename mtao::Vector<Scalar,3> Vector;
                SphereMeshFactory(int depth=3);
                void triforce(const Face & f, int depth);
                unsigned int add_edge(Edge e);
                void write(const std::string & filename);
                void write(std::ostream & outstream);
                const std::vector<Face> faces() const {return m_faces;}
                const std::vector<Vector> vertices() const {return m_vertices;}

            mtao::ColVectors<Scalar,3> V() const ;
            mtao::ColVectors<int,3> F() const ;
                private:
                const int m_depth = 0;
                mtao::vector<Vector> m_vertices;
                mtao::vector<Face> m_faces;
                std::map<Edge, unsigned int> m_edges;

            };

        template <typename T>
            SphereMeshFactory<T>::SphereMeshFactory(int depth): m_depth(depth) {
                //Create icosahedron base

                Scalar gr = .5 * (1 + std::sqrt(Scalar(5)));
                m_vertices.resize(12);

                m_vertices[ 0] = Vector(     0,    - 1,     gr);
                m_vertices[ 1] = Vector(    gr,      0,      1);
                m_vertices[ 2] = Vector(    gr,      0,    - 1);
                m_vertices[ 3] = Vector(   -gr,      0,    - 1);
                m_vertices[ 4] = Vector(   -gr,      0,      1);
                m_vertices[ 5] = Vector(   - 1,     gr,      0);
                m_vertices[ 6] = Vector(     1,     gr,      0);
                m_vertices[ 7] = Vector(     1,    -gr,      0);
                m_vertices[ 8] = Vector(   - 1,    -gr,      0);
                m_vertices[ 9] = Vector(     0,    - 1,    -gr);
                m_vertices[10] = Vector(     0,      1,    -gr);
                m_vertices[11] = Vector(     0,      1,     gr);
                for(auto&& v: m_vertices) {
                    v.normalize();
                }

                triforce({{ 1 ,  2 ,  6}},depth);
                triforce({{ 1 ,  7 ,  2}},depth);
                triforce({{ 3 ,  4 ,  5}},depth);
                triforce({{ 4 ,  3 ,  8}},depth);
                triforce({{ 6 ,  5 , 11}},depth);
                triforce({{ 5 ,  6 , 10}},depth);
                triforce({{ 9 , 10 ,  2}},depth);
                triforce({{10 ,  9 ,  3}},depth);
                triforce({{ 7 ,  8 ,  9}},depth);
                triforce({{ 8 ,  7 ,  0}},depth);
                triforce({{11 ,  0 ,  1}},depth);
                triforce({{ 0 , 11 ,  4}},depth);
                triforce({{ 6 ,  2 , 10}},depth);
                triforce({{ 1 ,  6 , 11}},depth);
                triforce({{ 3 ,  5 , 10}},depth);
                triforce({{ 5 ,  4 , 11}},depth);
                triforce({{ 2 ,  7 ,  9}},depth);
                triforce({{ 7 ,  1 ,  0}},depth);
                triforce({{ 3 ,  9 ,  8}},depth);
                triforce({{ 4 ,  8 ,  0}},depth);

            }
        template <typename T>
            void SphereMeshFactory<T>::triforce(const Face & f, int depth) {
                if(depth <= 0) {
                    m_faces.push_back(f);
                } else {
                    unsigned int e01 = add_edge({{f[0],f[1]}});
                    unsigned int e12 = add_edge({{f[1],f[2]}});
                    unsigned int e02 = add_edge({{f[0],f[2]}});
                    triforce({{f[0],e01,e02}},depth-1);
                    triforce({{f[1],e12,e01}},depth-1);
                    triforce({{f[2],e02,e12}},depth-1);
                    triforce({{e01 ,e12,e02}},depth-1);
                }

            }

        template <typename T>
            unsigned int SphereMeshFactory<T>::add_edge(Edge e) {
                if(e[0] > e[1]) {
                    unsigned int tmp = e[0];
                    e[0] = e[1];
                    e[1] = tmp;
                }
                auto it = m_edges.find(e);
                if(it != m_edges.end()) {
                    return it->second;
                } else {
                    m_edges[e] = m_vertices.size();
                    m_vertices.push_back(
                            (m_vertices[e[0]] + m_vertices[e[1]]).normalized()
                            );
                    return m_vertices.size()-1;
                }

            }

        template <typename T>
            mtao::ColVectors<T,3> SphereMeshFactory<T>::V() const {
            mtao::ColVectors<T,3> v(3,m_vertices.size());
                for(size_t i = 0; i < m_vertices.size(); ++i) {
                    v.col(i) = m_vertices[i];
                }
                return v;
            }

        template <typename T>
            mtao::ColVectors<int,3> SphereMeshFactory<T>::F() const {
                mtao::ColVectors<int,3> f(3,m_faces.size());
                for(size_t i = 0; i < m_faces.size(); ++i) {
                    f.col(i) = Eigen::Map<const mtao::Vector<unsigned int,3>>(m_faces[i].data()).cast<int>();
                }
                return f;
            }

    }

}
#endif


// Author: Tucker Haydon

#include <iostream>
#include <cstdlib>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <osqp.h>

#include "polynomial_solver.h"
#include "common.h"

namespace p4 {
  namespace {
    // Helper structure that contains pre-computed constants
    struct Constants {
      size_t num_dimensions;
      size_t polynomial_order;
      size_t derivative_order;
      size_t continuity_order;
      size_t num_intermediate_points;
      size_t num_nodes;
      size_t num_segments;
      size_t num_params_per_node_per_dim;
      size_t num_params_per_segment_per_dim;
      size_t num_params_per_node;
      size_t num_params_per_segment;
      size_t total_num_params;
      size_t num_constraints;
    };

    // Generates a square matrix that is the integrated form of d^n/dt^n [p(x)'p(x)].
    // The derivative of this matrix can be easily calculated by computing the
    // zeroth derivative of the matrix, padding the first n rows and columns
    // with zeros, and shifting the matrix down and to the right by n
    // rows/columns.
    //
    // See the theory documentation for further details.
    Eigen::MatrixXd QuadraticMatrix(
        const size_t polynomial_order,
        const size_t derivative_order,
        const double dt) {
      Eigen::MatrixXd base_integrated_quadratic_matrix;
      base_integrated_quadratic_matrix.resize(polynomial_order + 1, polynomial_order + 1);
      base_integrated_quadratic_matrix.fill(0);
      for(size_t row = 0; row < polynomial_order + 1; ++row) {
        for(size_t col = 0; col < polynomial_order + 1; ++col) {
          base_integrated_quadratic_matrix(row, col) =
            std::pow(dt, row + col + 1)
            / Factorial(row)
            / Factorial(col)
            / (row + col + 1);
        }
      }

      // Vector of ones
      Eigen::MatrixXd ones_vec;
      ones_vec.resize(polynomial_order + 1 - derivative_order, 1);
      ones_vec.fill(1);

      // Shift the matrix down rows
      Eigen::MatrixXd row_shift_mat;
      row_shift_mat.resize(polynomial_order + 1, polynomial_order + 1);
      row_shift_mat.fill(0);
      row_shift_mat.diagonal(-1*derivative_order) = ones_vec;

      // Shift the matrix right cols
      Eigen::MatrixXd col_shift_mat;
      col_shift_mat.resize(polynomial_order + 1, polynomial_order + 1);
      col_shift_mat.fill(0);
      col_shift_mat.diagonal(+1*derivative_order) = ones_vec;

      Eigen::MatrixXd integrated_quadratic_matrix;
      integrated_quadratic_matrix.resize(polynomial_order + 1, polynomial_order + 1);
      integrated_quadratic_matrix = row_shift_mat * base_integrated_quadratic_matrix * col_shift_mat;

      return integrated_quadratic_matrix;
    }

    // Sets the upper and lower bound vectors for the equality and continuity
    // constraints.
    void SetConstraints(
        const Constants& constants,
        const std::vector<double>& times,
        const std::vector<NodeEqualityBound>& explicit_node_equality_bounds,
        const std::vector<NodeInequalityBound>& explicit_node_inequality_bounds,
        const std::vector<SegmentInequalityBound>& explicit_segment_inequality_bounds,
        Eigen::MatrixXd& lower_bound_vec,
        Eigen::MatrixXd& upper_bound_vec,
        std::vector<Eigen::Triplet<double>>& constraint_triplets
        ) {
      size_t constraint_idx = 0;
      for(size_t dimension_idx = 0; dimension_idx < constants.num_dimensions; ++dimension_idx) {
        for(size_t node_idx = 0; node_idx < constants.num_nodes; ++node_idx) {
          for(size_t derivative_idx = 0; derivative_idx < constants.num_params_per_segment_per_dim; ++derivative_idx) {

            // Equality Constraints
            for(const NodeEqualityBound& bound: explicit_node_equality_bounds) {
              if(
                  false == (bound.node_idx == node_idx) ||
                  false == (bound.dimension_idx == dimension_idx) ||
                  false == (bound.derivative_idx == derivative_idx)) {
                continue;
              }
              else {
                const double alpha = node_idx+1 < constants.num_nodes ? (times[node_idx + 1] - times[node_idx]) : 1;

                // Bounds. Scaled by alpha. See documentation.
                lower_bound_vec(constraint_idx,0) = bound.value * std::pow(alpha, derivative_idx);
                upper_bound_vec(constraint_idx,0) = bound.value * std::pow(alpha, derivative_idx);

                // Constraints
                size_t parameter_idx = 0
                  + derivative_idx
                  + constants.num_params_per_node_per_dim * node_idx
                  + constants.num_params_per_node_per_dim * constants.num_nodes * dimension_idx;
                constraint_triplets.emplace_back(constraint_idx, parameter_idx, 1);

                constraint_idx++;
              }
            }

            // Node inequality bound constraints
            for(const NodeInequalityBound& bound: explicit_node_inequality_bounds) {
              if(
                  false == (bound.node_idx == node_idx) ||
                  false == (bound.dimension_idx == dimension_idx) ||
                  false == (bound.derivative_idx == derivative_idx)) {
                continue;
              }
              else {
                const double alpha = node_idx+1 < constants.num_nodes ? (times[node_idx + 1] - times[node_idx]) : 1;

                // Bounds. Scaled by alpha. See documentation.
                lower_bound_vec(constraint_idx,0) = bound.lower * std::pow(alpha, derivative_idx);
                upper_bound_vec(constraint_idx,0) = bound.upper * std::pow(alpha, derivative_idx);

                // Constraints
                size_t parameter_idx = 0
                  + derivative_idx
                  + constants.num_params_per_node_per_dim * node_idx
                  + constants.num_params_per_node_per_dim * constants.num_nodes * dimension_idx;
                constraint_triplets.emplace_back(constraint_idx, parameter_idx, 1);

                constraint_idx++;
              }
            }
          }

          // Continuity constraints
          if(node_idx < constants.num_segments) {
            const size_t num_continuity_constraints = constants.continuity_order + 1;
            constexpr double delta_t = 1.0;
            const double alpha_k = times[node_idx + 1] - times[node_idx];
            const double alpha_kp1 = node_idx + 2 < constants.num_nodes ? times[node_idx + 2] - times[node_idx + 1] : 1.0;

            for(size_t continuity_idx = 0; continuity_idx < num_continuity_constraints; ++continuity_idx) {
              // Bounds
              lower_bound_vec(constraint_idx,0) = 0;
              upper_bound_vec(constraint_idx,0) = 0;

              // Constraints. Scaled by alpha. See documentation.
              // Propagate the current node
              Eigen::MatrixXd segment_propagation_coefficients;
              segment_propagation_coefficients.resize(1, constants.num_params_per_segment_per_dim);
              segment_propagation_coefficients.fill(0);
              segment_propagation_coefficients
                = TimeVector(constants.polynomial_order, continuity_idx, delta_t).transpose()
                / std::pow(alpha_k, continuity_idx);

              // Minus the next node
              Eigen::MatrixXd segment_terminal_coefficients;
              segment_terminal_coefficients.resize(1, constants.num_params_per_segment_per_dim);
              segment_terminal_coefficients.fill(0);
              segment_terminal_coefficients(0,continuity_idx) = -1 / std::pow(alpha_kp1, continuity_idx);

              size_t current_segment_idx = 0
                // Get to the right dimension
                + constants.num_params_per_node_per_dim * constants.num_nodes * dimension_idx
                // Get to the right node
                + constants.num_params_per_node_per_dim * node_idx;
              size_t next_segment_idx = 0
                // Get to the right dimension
                + constants.num_params_per_node_per_dim * constants.num_nodes * dimension_idx
                // Get to the right node
                + constants.num_params_per_node_per_dim * (node_idx + 1);

              for(size_t param_idx = 0; param_idx < constants.num_params_per_segment_per_dim; ++param_idx) {
                constraint_triplets.emplace_back(
                    constraint_idx,
                    current_segment_idx + param_idx,
                    segment_propagation_coefficients(0, param_idx));
                // TODO: Just insert one terminal constraint
                constraint_triplets.emplace_back(
                    constraint_idx,
                    next_segment_idx + param_idx,
                    segment_terminal_coefficients(0, param_idx));
              }

              constraint_idx++;
            }
          }
        }
      }

      // Include start- and end-points in segment constraints. When constraining
      // a segment, also constrain the endpoints of the segment to the same
      // value. If this were not the case, the following situation could occur:
      // the start endpoint is constrained to -2, but the following segment is
      // constrained above zero. Clearly, there is no smooth solution that
      // permits this.
      //
      // Add two to account for the endpoints and then remove one to convert
      // from the number of points the the number of segments. Divide the
      // segment length (1) by the number of segments to get the length of each
      // intermediate segment.
      const double dt = 1.0 / (constants.num_intermediate_points + 2 - 1);

      // Segment lower bound constraints
      for(const SegmentInequalityBound& bound: explicit_segment_inequality_bounds) {
        const double alpha = times[bound.segment_idx+1] - times[bound.segment_idx];

        // point_idx == intermediate_point_idx
        // Add 2 for start and end points
        for(size_t point_idx = 0; point_idx < constants.num_intermediate_points+2; ++point_idx)  {
          // Bounds
          lower_bound_vec(constraint_idx,0) = -SegmentInequalityBound::INFTY;
          upper_bound_vec(constraint_idx,0) = bound.value * std::pow(alpha, bound.derivative_idx);

          // Time at a specific point
          double time = point_idx * dt;

          Eigen::MatrixXd segment_propagation_coefficients;
          segment_propagation_coefficients.resize(1, constants.num_params_per_segment_per_dim);
          segment_propagation_coefficients.fill(0);
          segment_propagation_coefficients
            = TimeVector(constants.polynomial_order, bound.derivative_idx, time).transpose();

          for(size_t dimension_idx = 0; dimension_idx < constants.num_dimensions; ++dimension_idx) {
            Eigen::MatrixXd transform_coefficients;
            transform_coefficients.resize(1, constants.num_params_per_segment_per_dim);
            transform_coefficients.fill(0);
            transform_coefficients = bound.mapping(dimension_idx, 0)
              * segment_propagation_coefficients;

            size_t current_segment_idx = 0
              // Get to the right dimension
              + constants.num_params_per_node_per_dim * constants.num_nodes * dimension_idx
              // Get to the right node
              + constants.num_params_per_segment_per_dim * bound.segment_idx;

            for(size_t param_idx = 0; param_idx < constants.num_params_per_segment_per_dim; ++param_idx) {
              constraint_triplets.emplace_back(
                  constraint_idx,
                  current_segment_idx + param_idx,
                  transform_coefficients(0, param_idx));
            }
          }

          constraint_idx++;
        }
      }
    }

    void SetQuadraticCost(
        const Constants& constants,
        std::vector<Eigen::Triplet<double>>& quadratic_triplets) {
      const double delta_t = 1.0;
      const Eigen::MatrixXd quadratic_matrix = QuadraticMatrix(constants.polynomial_order, constants.derivative_order, delta_t);

      for(size_t dimension_idx = 0; dimension_idx < constants.num_dimensions; ++dimension_idx) {
        // No cost for final node
        for(size_t node_idx = 0; node_idx < constants.num_nodes - 1; ++node_idx) {
          const size_t parameter_idx = 0
            // Get to the right dimension
            + constants.num_params_per_node_per_dim * constants.num_nodes * dimension_idx
            // Get to the right node
            + constants.num_params_per_node_per_dim * node_idx;
          for(size_t row = 0; row < constants.num_params_per_node_per_dim; ++row) {
            for(size_t col = 0; col < constants.num_params_per_node_per_dim; ++col) {
              quadratic_triplets.emplace_back(
                  row + parameter_idx,
                  col + parameter_idx,
                  quadratic_matrix(row,col)
                  );
            }
          }
        }
      }
    }

    // Converts an en eigen sparse matrix into an OSQP sparse matrix
    // Reference: https://github.com/robotology/osqp-eigen
    void Eigen2OSQP(
        const Eigen::SparseMatrix<double> eigen_sparse_mat,
        csc*& osqp_mat) {

      // get number of row, columns and nonZeros from Eigen SparseMatrix
      c_int rows   = eigen_sparse_mat.rows();
      c_int cols   = eigen_sparse_mat.cols();
      c_int num_nz = eigen_sparse_mat.nonZeros();

      // get inner and outer index
      const int* innerIndexPtr    = eigen_sparse_mat.innerIndexPtr();
      const int* outerIndexPtr    = eigen_sparse_mat.outerIndexPtr();
      const int* innerNonZerosPtr = eigen_sparse_mat.innerNonZeroPtr();

      // get nonzero values
      const double* valuePtr = eigen_sparse_mat.valuePtr();

      // Allocate memory for csc matrix
      if(osqp_mat != nullptr){
        std::cerr << "osqp_mat pointer is not a null pointer! " << std::endl;
        std::exit(EXIT_FAILURE);
      }

      osqp_mat = csc_spalloc(rows, cols, num_nz, 1, 0);

      int innerOsqpPosition = 0;
      for(int k = 0; k < cols; ++k) {
          if (eigen_sparse_mat.isCompressed()) {
              osqp_mat->p[k] = static_cast<c_int>(outerIndexPtr[k]);
          } else {
              if (k == 0) {
                  osqp_mat->p[k] = 0;
              } else {
                  osqp_mat->p[k] = osqp_mat->p[k-1] + innerNonZerosPtr[k-1];
              }
          }
          for (typename Eigen::SparseMatrix<double>::InnerIterator it(eigen_sparse_mat,k); it; ++it) {
              osqp_mat->i[innerOsqpPosition] = static_cast<c_int>(it.row());
              osqp_mat->x[innerOsqpPosition] = static_cast<c_float>(it.value());
              innerOsqpPosition++;
          }
      }
      osqp_mat->p[static_cast<int>(cols)] = static_cast<c_int>(innerOsqpPosition);
    }
  }

  PolynomialSolver::Solution PolynomialSolver::Run(
      const std::vector<double>& times,
      const std::vector<NodeEqualityBound>& explicit_node_equality_bounds,
      const std::vector<NodeInequalityBound>& explicit_node_inequality_bounds,
      const std::vector<SegmentInequalityBound>& explicit_segment_inequality_bounds) {

    this->options_.Check();

    if(times.size() < 2) {
      std::cerr << "PolynomialSolver::Run -- Time vector must have a size greater than one." << std::endl;
      std::exit(EXIT_FAILURE);
    }

    Constants constants;
    constants.num_dimensions = this->options_.num_dimensions;
    constants.polynomial_order = this->options_.polynomial_order;
    constants.derivative_order = this->options_.derivative_order;
    constants.continuity_order = this->options_.continuity_order;
    constants.num_intermediate_points = this->options_.num_intermediate_points;
    constants.num_nodes = times.size();
    constants.num_segments = constants.num_nodes - 1;
    constants.num_params_per_node_per_dim = constants.polynomial_order + 1;
    constants.num_params_per_segment_per_dim = constants.polynomial_order + 1;
    constants.num_params_per_node = constants.num_dimensions * constants.num_params_per_node_per_dim;
    constants.num_params_per_segment = constants.num_dimensions * constants.num_params_per_segment_per_dim;
    constants.total_num_params = constants.num_params_per_node * constants.num_nodes;

    // Explicit constraints are provided
    const size_t num_explicit_constraints = 0
      + explicit_node_equality_bounds.size()
      + explicit_node_inequality_bounds.size()
      + explicit_segment_inequality_bounds.size() * (constants.num_intermediate_points+2);

    // Implicit constraints are continuity constraints
    const size_t num_implicit_constraints = constants.num_segments*(constants.continuity_order+1)*constants.num_dimensions;

    constants.num_constraints = num_explicit_constraints + num_implicit_constraints;

    /*
     * CONSTRAINTS
     */
    Eigen::MatrixXd lower_bound_vec, upper_bound_vec;
    lower_bound_vec.resize(constants.num_constraints, 1);
    upper_bound_vec.resize(constants.num_constraints, 1);

    std::vector<Eigen::Triplet<double>> constraint_triplets;
    SetConstraints(
        constants,
        times,
        explicit_node_equality_bounds,
        explicit_node_inequality_bounds,
        explicit_segment_inequality_bounds,
        lower_bound_vec,
        upper_bound_vec,
        constraint_triplets);

    // Triplets to sparse mat
    Eigen::SparseMatrix<double> sparse_constraint_mat(
        constants.num_constraints,
        constants.total_num_params);
    sparse_constraint_mat.setFromTriplets(
        constraint_triplets.begin(),
        constraint_triplets.end());

    /*
     * QUADRATIC MATRIX
     */
    std::vector<Eigen::Triplet<double>> quadratic_triplets;
    SetQuadraticCost(constants, quadratic_triplets);

    // Triplets to sparse mat
    Eigen::SparseMatrix<double> sparse_quadratic_mat(
        constants.total_num_params,
        constants.total_num_params);
    sparse_quadratic_mat.setFromTriplets(
        quadratic_triplets.begin(),
        quadratic_triplets.end());

    /*
     * CONVERT EIGEN TO OSQP
     */
    csc* P = nullptr;
    csc* A = nullptr;

    Eigen2OSQP(sparse_quadratic_mat, P);
    Eigen2OSQP(sparse_constraint_mat, A);

    c_float q[constants.total_num_params];
    for(size_t param_idx = 0; param_idx < constants.total_num_params; ++param_idx) {
      q[param_idx] = 0;
    }

    c_float l[constants.num_constraints], u[constants.num_constraints];
    for(size_t row_idx = 0; row_idx < constants.num_constraints; ++row_idx) {
      l[row_idx] = lower_bound_vec(row_idx, 0);
      u[row_idx] = upper_bound_vec(row_idx, 0);
    }

    /*
     * RUN THE SOLVER
     */
    // Allocate and populate data
    std::shared_ptr<OSQPData> data = std::shared_ptr<OSQPData>(
        (OSQPData *)c_malloc(sizeof(OSQPData)),
        [](OSQPData* data) {
          c_free(data->A);
          c_free(data->P);
        });
    data->n = constants.total_num_params;
    data->m = constants.num_constraints;
    data->P = P;
    data->q = q;
    data->A = A;
    data->l = l;
    data->u = u;

    // Allocate and prepare workspace
    // Workspace shared pointer requires custom destructor
    PolynomialSolver::Solution solution;
    solution.num_dimensions   = constants.num_dimensions;
    solution.polynomial_order = constants.polynomial_order;
    solution.num_nodes        = constants.num_nodes;
    solution.workspace =  std::shared_ptr<OSQPWorkspace>(
        osqp_setup(data.get(), &this->options_.osqp_settings),
        [](OSQPWorkspace* workspace) {
          osqp_cleanup(workspace);
        });

    // Solve
    osqp_solve(solution.workspace.get());

    // Return the solution
    return solution;
  }

  void PolynomialSolver::Options::Check() {
    if(this->num_dimensions < 1) {
      std::cerr << "PolynomialSolver::Options::Check -- Number of dimensions must be greater than zero." << std::endl;
      std::exit(EXIT_FAILURE);
    }
  }

  std::vector<std::vector<Eigen::VectorXd>> PolynomialSolver::Solution::Coefficients() const {
    const size_t num_segments                   = this->num_nodes - 1;
    const size_t num_params_per_node_per_dim    = this->polynomial_order + 1;
    const size_t num_params_per_segment_per_dim = this->polynomial_order + 1;
    const size_t num_params_per_node            = this->num_dimensions * num_params_per_node_per_dim;
    const size_t num_params_per_segment         = this->num_dimensions * num_params_per_segment_per_dim;
    const size_t total_num_params               = this->num_nodes * num_params_per_node;

    std::vector<std::vector<Eigen::VectorXd>> coefficients;

    coefficients.resize(this->num_dimensions);
    for(size_t dimension_idx = 0; dimension_idx < this->num_dimensions; ++dimension_idx) {
      coefficients[dimension_idx].resize(this->num_nodes);
      for(size_t node_idx = 0; node_idx < this->num_nodes; ++node_idx) {
        coefficients[dimension_idx][node_idx].resize(num_params_per_node_per_dim);
        for(size_t coefficient_idx = 0; coefficient_idx < num_params_per_node_per_dim; ++coefficient_idx) {
          const size_t parameter_idx = 0
            // Get to the right dimension
            + num_params_per_node_per_dim * this->num_nodes * dimension_idx
            // Get to the right node
            + num_params_per_node_per_dim * node_idx
            // Get to the right parameter idx
            + coefficient_idx;

          coefficients[dimension_idx][node_idx](coefficient_idx)
            = this->workspace->solution->x[parameter_idx];
        }
      }
    }
    return coefficients;
  }

  Eigen::VectorXd PolynomialSolver::Solution::Coefficients(
      const size_t dimension_idx,
      const size_t node_idx) const {
    const size_t num_segments                   = this->num_nodes - 1;
    const size_t num_params_per_node_per_dim    = this->polynomial_order + 1;
    const size_t num_params_per_segment_per_dim = this->polynomial_order + 1;
    const size_t num_params_per_node            = this->num_dimensions * num_params_per_node_per_dim;
    const size_t num_params_per_segment         = this->num_dimensions * num_params_per_segment_per_dim;
    const size_t total_num_params               = this->num_nodes * num_params_per_node;

    Eigen::VectorXd coefficients;
    coefficients.resize(num_params_per_node_per_dim);
    for(size_t coefficient_idx = 0; coefficient_idx < num_params_per_node_per_dim; ++coefficient_idx) {
      const size_t parameter_idx = 0
        // Get to the right dimension
        + num_params_per_node_per_dim * this->num_nodes * dimension_idx
        // Get to the right node
        + num_params_per_node_per_dim * node_idx
        // Get to the right parameter idx
        + coefficient_idx;

      coefficients(coefficient_idx)
        = this->workspace->solution->x[parameter_idx];
    }

    return coefficients;
  }
}