#ifndef OPENCV_RGBD_GRAPH_NODE_H
#define OPENCV_RGBD_GRAPH_NODE_H

#include <map>
#include <unordered_map>

#include "opencv2/core/affine.hpp"
#if defined(HAVE_EIGEN)
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "opencv2/core/eigen.hpp"
#endif

#if defined(CERES_FOUND)
#include <ceres/ceres.h>
#endif

namespace cv
{
namespace kinfu
{
/*! \class GraphNode
 *  \brief Defines a node/variable that is optimizable in a posegraph
 *
 *  Detailed description
 */
#if defined(HAVE_EIGEN)
struct Pose3d
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Eigen::Vector3d t;
    Eigen::Quaterniond r;

    Pose3d()
    {
        t.setZero();
        r.setIdentity();
    };
    Pose3d(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation)
        : t(translation), r(Eigen::Quaterniond(rotation))
    {
        normalizeRotation();
    }

    Pose3d(const Matx33d& rotation, const Vec3d& translation)
    {
        Eigen::Matrix3d R;
        cv2eigen(rotation, R);
        cv2eigen(translation, t);
        r = Eigen::Quaterniond(R);
        normalizeRotation();
    }

    explicit Pose3d(const Matx44f& pose)
    {
        Matx33d rotation(pose.val[0], pose.val[1], pose.val[2], pose.val[4], pose.val[5],
                         pose.val[6], pose.val[8], pose.val[9], pose.val[10]);
        Vec3d translation(pose.val[3], pose.val[7], pose.val[11]);
        Pose3d(rotation, translation);
    }

    // NOTE: Eigen overloads quaternion multiplication appropriately
    inline Pose3d operator*(const Pose3d& otherPose) const
    {
        Pose3d out(*this);
        out.t += r * otherPose.t;
        out.r *= otherPose.r;
        out.normalizeRotation();
        return out;
    }

    inline Pose3d& operator*=(const Pose3d& otherPose)
    {
        t += otherPose.t;
        r *= otherPose.r;
        normalizeRotation();
        return *this;
    }

    inline Pose3d inverse() const
    {
        Pose3d out;
        out.r = r.conjugate();
        out.t = out.r * (t * -1.0);
        return out;
    }

    inline void normalizeRotation()
    {
        if (r.w() < 0)
            r.coeffs() *= -1.0;
        r.normalize();
    }
};
#endif

struct PoseGraphNode
{
   public:
    explicit PoseGraphNode(int _nodeId, const Affine3f& _pose)
        : nodeId(_nodeId), isFixed(false), pose(_pose)
    {
#if defined(HAVE_EIGEN)
        se3Pose = Pose3d(_pose.rotation(), _pose.translation());
#endif
    }
    virtual ~PoseGraphNode() = default;

    int getId() const { return nodeId; }
    inline Affine3f getPose() const
    {
        return pose;
    }
    void setPose(const Affine3f& _pose)
    {
        pose = _pose;
#if defined(HAVE_EIGEN)
        se3Pose = Pose3d(pose.rotation(), pose.translation());
#endif
    }
#if defined(HAVE_EIGEN)
    void setPose(const Pose3d& _pose)
    {
        se3Pose = _pose;
        const Eigen::Matrix3d& rotation    = se3Pose.r.toRotationMatrix();
        const Eigen::Vector3d& translation = se3Pose.t;
        Matx33d rot;
        Vec3d trans;
        eigen2cv(rotation, rot);
        eigen2cv(translation, trans);
        Affine3d poseMatrix(rot, trans);
        pose = poseMatrix;
    }
#endif
    void setFixed(bool val = true) { isFixed = val; }
    bool isPoseFixed() const { return isFixed; }

   public:
    int nodeId;
    bool isFixed;
    Affine3f pose;
#if defined(HAVE_EIGEN)
    Pose3d se3Pose;
#endif
};

/*! \class PoseGraphEdge
 *  \brief Defines the constraints between two PoseGraphNodes
 *
 *  Detailed description
 */
struct PoseGraphEdge
{
   public:
    PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation,
                  const Matx66f& _information = Matx66f::eye())
        : sourceNodeId(_sourceNodeId),
          targetNodeId(_targetNodeId),
          transformation(_transformation),
          information(_information)
    {
    }
    virtual ~PoseGraphEdge() = default;

    int getSourceNodeId() const { return sourceNodeId; }
    int getTargetNodeId() const { return targetNodeId; }

    bool operator==(const PoseGraphEdge& edge)
    {
        if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) ||
            (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId))
            return true;
        return false;
    }

   public:
    int sourceNodeId;
    int targetNodeId;
    Affine3f transformation;
    Matx66f information;
};

//! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold
//! optimization Jose Luis Blanco Compactly represents the jacobian of the SE3 generator
// clang-format off
/* static const std::array<Matx44f, 6> generatorJacobian = { */
/*     // alpha */
/*     Matx44f(0, 0,  0, 0, */
/*             0, 0, -1, 0, */
/*             0, 1,  0, 0, */
/*             0, 0,  0, 0), */
/*     // beta */
/*     Matx44f( 0, 0, 1, 0, */
/*              0, 0, 0, 0, */
/*             -1, 0, 0, 0, */
/*              0, 0, 0, 0), */
/*     // gamma */
/*     Matx44f(0, -1, 0, 0, */
/*             1,  0, 0, 0, */
/*             0,  0, 0, 0, */
/*             0,  0, 0, 0), */
/*     // x */
/*     Matx44f(0, 0, 0, 1, */
/*             0, 0, 0, 0, */
/*             0, 0, 0, 0, */
/*             0, 0, 0, 0), */
/*     // y */
/*     Matx44f(0, 0, 0, 0, */
/*             0, 0, 0, 1, */
/*             0, 0, 0, 0, */
/*             0, 0, 0, 0), */
/*     // z */
/*     Matx44f(0, 0, 0, 0, */
/*             0, 0, 0, 0, */
/*             0, 0, 0, 1, */
/*             0, 0, 0, 0) */
/* }; */
// clang-format on

class PoseGraph
{
   public:
    typedef std::vector<PoseGraphNode> NodeVector;
    typedef std::vector<PoseGraphEdge> EdgeVector;

    explicit PoseGraph(){};
    virtual ~PoseGraph() = default;

    //! PoseGraph can be copied/cloned
    PoseGraph(const PoseGraph&) = default;
    PoseGraph& operator=(const PoseGraph&) = default;

    void addNode(const PoseGraphNode& node) { nodes.push_back(node); }
    void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); }

    bool nodeExists(int nodeId) const
    {
        return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) {
                   return currNode.getId() == nodeId;
               }) != nodes.end();
    }

    bool isValid() const;

    int getNumNodes() const { return int(nodes.size()); }
    int getNumEdges() const { return int(edges.size()); }

   public:
    NodeVector nodes;
    EdgeVector edges;
};

namespace Optimizer
{
void optimize(PoseGraph& poseGraph);

#if defined(CERES_FOUND)
void createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem);

//! Error Functor required for Ceres to obtain an auto differentiable cost function
class Pose3dErrorFunctor
{
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Pose3dErrorFunctor(const Pose3d& _poseMeasurement, const Matx66d& _sqrtInformation)
        : poseMeasurement(_poseMeasurement)
    {
        cv2eigen(_sqrtInformation, sqrtInfo);
    }
    Pose3dErrorFunctor(const Pose3d& _poseMeasurement,
                       const Eigen::Matrix<double, 6, 6>& _sqrtInformation)
        : poseMeasurement(_poseMeasurement), sqrtInfo(_sqrtInformation)
    {
    }

    template<typename T>
    bool operator()(const T* const _pSourceTrans, const T* const _pSourceQuat,
                    const T* const _pTargetTrans, const T* const _pTargetQuat, T* _pResidual) const
    {
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> sourceTrans(_pSourceTrans);
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> targetTrans(_pTargetTrans);
        Eigen::Map<const Eigen::Quaternion<T>> sourceQuat(_pSourceQuat);
        Eigen::Map<const Eigen::Quaternion<T>> targetQuat(_pTargetQuat);
        Eigen::Map<Eigen::Matrix<T, 6, 1>> residual(_pResidual);

        Eigen::Quaternion<T> targetQuatInv = targetQuat.conjugate();

        Eigen::Quaternion<T> relativeQuat    = targetQuatInv * sourceQuat;
        Eigen::Matrix<T, 3, 1> relativeTrans = targetQuatInv * (targetTrans - sourceTrans);

        //! Definition should actually be relativeQuat * poseMeasurement.r.conjugate()
        Eigen::Quaternion<T> deltaRot =
            poseMeasurement.r.template cast<T>() * relativeQuat.conjugate();

        residual.template block<3, 1>(0, 0) = relativeTrans - poseMeasurement.t.template cast<T>();
        residual.template block<3, 1>(3, 0) = T(2.0) * deltaRot.vec();

        residual.applyOnTheLeft(sqrtInfo.template cast<T>());

        return true;
    }

    static ceres::CostFunction* create(const Pose3d& _poseMeasurement,
                                       const Matx66f& _sqrtInformation)
    {
        return new ceres::AutoDiffCostFunction<Pose3dErrorFunctor, 6, 3, 4, 3, 4>(
            new Pose3dErrorFunctor(_poseMeasurement, _sqrtInformation));
    }

   private:
    const Pose3d poseMeasurement;
    Eigen::Matrix<double, 6, 6> sqrtInfo;
};
#endif

}  // namespace Optimizer

}  // namespace kinfu
}  // namespace cv
#endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */


#include <iostream>
#include <cmath>
#include <chrono>
#include <cassert>
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/vector.h>
#include "Element.hpp"

using namespace std;
using namespace std::chrono;
using namespace NTL;

void test_wilson(int bound);
void test_wolstenholme(int bound);
void test_kurepa(int bound);

template<typename T, typename M>
void remainder_tree(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<T>> &m, Elt<T> &AProd, Elt<M> &mProd, Elt<T> const &root_value = Elt<T>(1), int start = 0, int end = -1);
// void remainder_tree_v1(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> const &root_value = Elt<T>(1), const int k = 2);
template<typename T, typename M>
Elt<T> get_node(int index, vector<Elt<T>> &base, Elt<M> const &mod = Elt<M>(0));
template<typename T, typename M>
void remainder_tree_v2(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> const &root_value = Elt<T>(1), const int k = 2);
template<typename T>
void print_tree(vector<Elt<T>> tree);
void complexity_graph(int N, int d);

/* Tags:
 *
 * //DEBUG// for debug statements
 * Optimization idea for optimizations that havent been impemented yet
 * TODO: add typename specifications to functions when they are called
 * TODO: add .t whenever need to access a value of Elt
 */

int main(){

	complexity_graph(1<<24, 10);
}

template<typename T>
void elt_to_base(vector<T> &b, vector<Elt<T>> &e){
	assert(b.size() == e.size());
	for(int i = 0; i < b.size(); i++){
		b[i] = e[i].t;
	}
}

template<typename T>
void base_to_elt(vector<Elt<T>> &e, vector<T> &b){
	assert(e.size() == b.size());
	for(int i = 0; i < e.size(); i++){
		e[i] = b[i];
	}
}

// Test for Wilson Primes
void test_wilson(int bound){

	vector<ZZ> A;
	A.resize(bound);
	vector<ZZ> m;
	m.resize(bound);

	vector<Elt<ZZ>> A_elt;
	A_elt.resize(bound);
	vector<Elt<ZZ>> m_elt;
	m_elt.resize(bound);

	base_to_elt(A_elt, A);
	base_to_elt(m_elt, m);

	// make sure implicit ints don't overflow
	for(int i = 1; i <= bound; i++){
		A[i-1] = ZZ(i);
		m[i-1] = ProbPrime(ZZ(i)) ? ZZ(i)*ZZ(i) : ZZ(1);
	}

	/*
	for(int i = 0; i < A.size(); i++){
		cout << A[i] << " ";
	}
	cout << endl;

	for(int i = 0; i < m.size(); i++){
		cout << m[i] << " ";
	}
	cout << endl;
	*/

	vector<ZZ> C;
	C.resize(bound);
	vector<Elt<ZZ>> C_elt;
	C_elt.resize(bound);

	base_to_elt(C_elt, C);

	remainder_tree_v2<ZZ, ZZ>(C_elt, A_elt, m_elt, ZZ(1), 4);

	elt_to_base(C, C_elt);

	/*
	for(int i = 0; i < C.size(); i++){
		cout << (i+1) << ": " << C[i] << endl;
	}
	cout << endl;
	*/
}

// Test for Wolstenholme Primes
void test_wolstenholme(int bound){

	vector<ZZ> Anum;
	Anum.resize(bound);
	vector<ZZ> m;
	m.resize(bound);

	vector<Elt<ZZ>> Anum_elt;
	Anum_elt.resize(bound);
	vector<Elt<ZZ>> m_elt;
	m_elt.resize(bound);

	for(int i = 1; i <= bound; i++){
		Anum[i-1] = 4*i+2;
		m[i-1] = ProbPrime(ZZ(i)) ? ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i) : ZZ(1);

	}

	base_to_elt(Anum_elt, Anum);
	base_to_elt(m_elt, m);

	vector<ZZ> Cnum;
	Cnum.resize(bound);
	vector<Elt<ZZ>> Cnum_elt;
	Cnum_elt.resize(bound);

	remainder_tree_v2<ZZ, ZZ>(Cnum_elt, Anum_elt, m_elt, ZZ(1), 4);

	elt_to_base(Cnum, Cnum_elt);

	for(int i = 1; i <= bound; i++){
		Cnum[i-1] /= i; // i = prime at this index
	}

	vector<ZZ> Adem;
	Adem.resize(bound);
	vector<Elt<ZZ>> Adem_elt;
	Adem_elt.resize(bound);

	for(int i = 1; i <= bound; i++){
		Adem[i-1] = i+1;
	}

	base_to_elt(Adem_elt, Adem);

	vector<ZZ> Cdem;
	Cdem.resize(bound);
	vector<Elt<ZZ>> Cdem_elt;
	Cdem_elt.resize(bound);

	remainder_tree_v2<ZZ, ZZ>(Cdem_elt, Adem_elt, m_elt, ZZ(1), 4);

	elt_to_base(Cdem, Cdem_elt);

	for(int i = 1; i <= bound; i++){
		Cdem[i-1] /= i;
		ZZ d;
		ZZ k;
		XGCD(d, Cdem[i-1], k, Cdem[i-1], ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i)); // changes Cdem[i-1] to its inverse mod p^4
		if (Cdem[i-1] < 0) Cdem[i-1] += ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i); // make residue positive

		Cnum[i-1] = (Cnum[i-1] * Cdem[i-1]) % (ZZ(i)*ZZ(i)*ZZ(i)*ZZ(i));
	}

	for(int i = 1; i <= bound; i++){
		if (Cnum[i-1] == 1){
			cout << i << ": " << Cnum[i-1] << endl;
		}
	}

}

void test_kurepa(int bound){
	// need to first implement remainder tree for matrices
}

/*
 * Original Remainder Tree implementation
 */

template<typename T, typename M>
void remainder_tree(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> &AProd, Elt<M> &mProd, Elt<T> const &root_value, int start, int end){

	//DEBUG// cout << "AProd: " << AProd << endl;
	//DEBUG// cout << "mProd: " << mProd << endl;
	//DEBUG// cout << "root_value: " << root_value << endl;
	// set default value for end
	if (end == -1) end = C.size();

	// Assert that interval [start, end] exists in C, A and m
	assert(end <= C.size());
	assert(end <= A.size());
	assert(end <= m.size());

	// Set N = length of interval
	int N = end - start;

	// Change nothing if N = 0
	if (N == 0) {
		return;
	}

	// Index of leaf at the bottom left
	int leftmost = 1 << ((int)ceil(log2(N)));

	// Declare trees (always of length 2N for any N)
	vector<Elt<T>> ATree;
	ATree.resize(2 * N);
	vector<Elt<M>> mTree;
	mTree.resize(2 * N);
	vector<Elt<T>> CTree;
	CTree.resize(2 * N);

	/*
	 * For example when N=11 the leaves are in this order:
	 *     / \       /\   /\    /\
	 *    /   \     /  7 8  9 10  11
	 *   /\   /\   /\
	 *  1  2 3  4 5  6
	 *
	 */

	// Initialize the leaves in ATree and mTree
	for (int i = leftmost; i < 2 * N; i++) { // leaves on lowest layer
		ATree[i] = A[i - leftmost + start];
		mTree[i] = m[i - leftmost + start];
	}
	for (int i = N; i < leftmost; i++) { // leaves on second lowest layer
		ATree[i] = A[i + N - leftmost + start];
		mTree[i] = m[i + N - leftmost + start];
	}

	// Calculate the rest of the product tree mTree
	for (int i = N - 1; i > 0; i--) {
		mTree[i] = mTree[2 * i] * mTree[2 * i + 1]; // parent is product of leaves
	}

	// Calculate the rest of the product tree aTree, taking mod mTree[1] = m[0]*...*m[N-1]
	for(int i = N - 1; i > 0; i--) {
		ATree[i] = (ATree[2 * i] * ATree[2 * i + 1]) % mProd; // parent is product of leaves mod mTree[1]
		ATree[2 * i] %= mTree[1];
		delete ATree[2 * i + 1];
	}

	mProd /= mTree[1]; // Get rid of this tree's moduli from mProd
	AProd = ATree[1]; // Set AProd as the product of A's mod new mProd

	// Calculate accumulating remainder tree
	CTree[1] = root_value % mTree[1];
	//DEBUG// cout << "CTree root: " << CTree[1] << endl;
	for (int i = 1; i < N; i++) {
		CTree[2 * i] = CTree[i] % mTree[2 * i]; // Left branch
		CTree[2 * i + 1] = (CTree[i] * ATree[2 * i]) % mTree[2 * i + 1]; // Right branch
		delete CTree[i];
	}

	//DEBUG// print_tree<ZZ>(ATree);
	//DEBUG// print_tree<ZZ>(mTree);
	//DEBUG// print_tree<ZZ>(CTree);

	for (int i = leftmost; i < 2 * N; i++) {
		C[i - leftmost + start] = CTree[i];
	}
	for (int i = N; i < leftmost; i++) {
		C[i + N - leftmost + start] = CTree[i];
	}

	return;
}

/*
 * Implements Sutherland's optimization
 * Doesn't do intervals yet
 * k = layer where we divide into subtrees
 */
template<typename T, typename M>
void remainder_tree_v2(vector<Elt<T>> &C, vector<Elt<T>> &A, vector<Elt<M>> &m, Elt<T> const &root_value, const int k){

	// Assert that lengths of A and m match
	assert(C.size() == A.size());
	assert(C.size() == m.size());

	// Set N = length of input arrays
	int N = C.size();

	// Change nothing if N = 0
	if (N == 0) {
		return;
	}

	// Ensure that there are at least k layers
	assert(N >= (1<<k));

	// Index of leaf at the bottom left
	int leftmost = 1 << ((int)ceil(log2(N)));

	// Declare Ctree (always of length 2N for any N)
	vector<Elt<T>> CTree;
	CTree.resize(2 * N);

	/*
	 * For example when N=11 the leaves are in this order:
	 *     / \       /\   /\    /\
	 *    /   \     /  7 8  9 10  11
	 *   /\   /\   /\
	 *  1  2 3  4 5  6
	 *
	 */

	// Calculate the product of all the mods to keep A's small
	// Elt<M> mProd = get_node<ZZ, ZZ>(1, m, Elt<M>(0));

	uint64_t start2 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

	// Step 2: Calculate the subproduct trees
	// Roots are CTree[2^k + i]: {CTree[2^k], ..., CTree[2^(k+1)-1]}

	// First find index of root in subtree with leaves in both layers
	int notfirstk = ((int)log2(2*N-1) - k); // (#bits in 2*N-1) minus k
	int special = (2*N-1 - leftmost) >> notfirstk; // firstk digits excluding the most significant digit

	Elt<T> AProd = Elt<T>(1);
	Elt<M> mProd = get_node<ZZ, ZZ>(1, m, Elt<M>(0));
	CTree[1<<k] = root_value % mProd;
	// Subtrees with leaves in first layer
	for(int i = 0; i < special; i++) {
		// Number of leaves: 2^notfirstk = leftmost/2^k
		//DEBUG// cout << "Calculating interval: [" << (i<<notfirstk) << ", " << ((i+1)<<notfirstk) << "]" << endl;
		remainder_tree<ZZ, ZZ>(C, A, m, AProd, mProd, CTree[(1<<k) + i], i<<notfirstk, (i+1)<<notfirstk);
		CTree[(1<<k) + i+1] = (CTree[(1<<k) + i] * AProd) % mProd;
	}

	// Subtree with leaves in both layers
	// First, calculate number of leaves in this subtree, stored in specialleaves
	int notfirstkdigits = (2*N-1) % (1<<notfirstk);
	int onenotfirstkdigits = (1<<notfirstk) + notfirstkdigits;
	int specialleaves = (onenotfirstkdigits+1)/2;
	//DEBUG// cout << "Calculating interval: [" << (special<<notfirstk) << ", " << ((special<<notfirstk) + specialleaves) << "]" << endl;
	remainder_tree<ZZ, ZZ>(C, A, m, AProd, mProd, CTree[(1<<k) + special], special<<notfirstk, (special<<notfirstk) + specialleaves);
	CTree[(1<<k) + special+1] = (CTree[(1<<k) + special] * AProd) % mProd;

	// Subtrees with leaves in second layer
	for(int i = special+1; i < 1<<k; i++){
		//DEBUG// cout << "Calculating interval: [" << ((special<<notfirstk) + specialleaves + ((i - special-1)<<(notfirstk-1))) << ", " << ((special<<notfirstk) + specialleaves + ((i - special)<<(notfirstk-1))) << "]" << endl;
		remainder_tree<ZZ, ZZ>(C, A, m, AProd, mProd, CTree[(1<<k) + i], (special<<notfirstk) + specialleaves + ((i - special-1)<<(notfirstk-1)), (special<<notfirstk) + specialleaves + ((i - special)<<(notfirstk-1)));
		if (i == (1<<k) - 1) continue; // Prevent index out of range for next operation
		CTree[(1<<k) + i+1] = (CTree[(1<<k) + i] * AProd) % mProd;
	}

	uint64_t end2 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
	//DEBUG// cout << "Time taken for subtree step: " << (end2-start2) << endl;

	return;

}

/*
 * Returns the value of the node on the tree at index k with leaves having value base
 */
template<typename T, typename M>
Elt<T> get_node(int i, vector<Elt<T>> &base, Elt<M> const &mod) { // Optimization idea: pass in what you're taking a mod of as well so if the modulus ever gets bigger than the value, just return the value
	int N = base.size();
	int leftmost = 1 << ((int)ceil(log2(N)));
	if (mod == 0){
		if (i >= leftmost) return base[i - leftmost];
		else if (i >= N) return base[i + N - leftmost];

		return get_node<ZZ, ZZ>(2*i, base, Elt<M>(0))*get_node<ZZ, ZZ>(2*i+1, base, Elt<M>(0));
	}

	else {
		if (i >= leftmost) return base[i - leftmost] % mod;
		else if (i >= N) return base[i + N - leftmost] % mod;

		return (get_node<ZZ, ZZ>(2*i, base, mod)*get_node<ZZ, ZZ>(2*i+1, base, mod)) % mod;
	}
}

/*
 * Prints a tree given in vector<ZZ> form
 */
template<typename T>
void print_tree(vector<Elt<T>> tree){
	int top = 1;
	int counter = 0;
	for(int i = 1; i < tree.size(); i++){
		cout << tree[i] << " ";
		counter++;
		if (counter == top){
			cout << endl;
			counter = 0;
			top *= 2;
		}
	}
	cout << endl;
}

/*
 * Gives data points on size of input vs. computation time.
 * N = max size of data, d = number of data points
 */

void complexity_graph(int N, int d){
	vector<int> x;
	vector<int> y;
	vector<int> z;

	int interval = N/d;
	int B = 0;
	while(B <= N){
		cout << "Testing: " << B << endl;
		/*
		int testSize = B;
		int numSize = B;

		vector<ZZ> test_A;
		test_A.resize(testSize);
		vector<ZZ> test_m;
		test_m.resize(testSize);
		for (int i = 0; i < testSize; i++) {
			test_A[i] = rand() % numSize + 1;
			test_m[i] = rand() % numSize + 1;
		}

		vector<ZZ> test_C;
		test_C.resize(testSize);
		*/

		x.push_back(B);

		uint64_t start;
		uint64_t end;

		start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
		//remainder_tree<ZZ, ZZ>(test_C, test_A, test_m);
		end = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

		y.push_back(end-start);

		start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
		test_wilson(B);
		end = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

		z.push_back(end-start);

		B += interval;
	}

	for(int i = 0; i < x.size(); i++){
		cout << x[i] << ", ";
	}
	cout << endl;
	for(int i = 0; i < y.size(); i++){
		cout << y[i] << ", ";
	}
	cout << endl;
	for(int i = 0; i < z.size(); i++){
		cout << z[i] << ", ";
	}
	cout << endl;

}


// File:  main.cpp
// Date:  11/17/2019
// Auth:  K. Loux
// Desc:  Entry point for SplinePatchToFlatPattern application.

// optimization headers
#include "optimization/nelderMead.h"

// Eigen headers
#include <Eigen/Eigen>
#include <Eigen/StdVector>

// Standard C++ headers
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include <iostream>
#include <cassert>
#include <algorithm>

typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> Vector2DVectors;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> Vector3DVectors;

bool ParseToken(const std::string& token, double& value)
{
	std::istringstream ss(token);
	return !(ss >> value).fail();
}

bool ParseLine(const std::string& line, Vector3DVectors& curve1, Vector3DVectors& curve2)
{
	std::istringstream ss(line);
	Eigen::Vector3d p1, p2;
	std::string token;
	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p1(0)))
		return false;

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p1(1)))
		return false;
	p1(1) = fabs(p1(1));

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p1(2)))
		return false;

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p2(0)))
		return false;

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p2(1)))
		return false;
	p2(1) = fabs(p2(1));

	if (!std::getline(ss, token, ','))
		return false;

	if (!ParseToken(token, p2(2)))
		return false;

	curve1.push_back(p1);
	curve2.push_back(p2);

	return true;
}

bool ReadInputFile(const std::string& fileName, Vector3DVectors& curve1, Vector3DVectors& curve2)
{
	std::ifstream file(fileName);
	if (!file.is_open() || !file.good())
	{
		std::cerr << "Failed to open '" << fileName << "' for input\n";
		return false;
	}

	std::string line;
	unsigned int lineCount(0);
	while (std::getline(file, line))
	{
		++lineCount;
		if (!ParseLine(line, curve1, curve2))
		{
			std::cerr << "Failed to parse line " << line << '\n';
			return false;
		}
	}

	auto sortPredicate([](const Eigen::Vector3d&a, const Eigen::Vector3d& b)
	{
		return a(0) < b(0);
	});

	std::sort(curve1.begin(), curve1.end(), sortPredicate);
	std::sort(curve2.begin(), curve2.end(), sortPredicate);

	return true;
}

class Spline
{
public:
	void AddPoint(const Eigen::Vector3d& p, const Eigen::Vector3d& c)
	{
		intersectionPoints.push_back(p);
		controlVectors.push_back(c);
	}

	void SetControlVector(const unsigned int& i, const Eigen::Vector3d& v) { controlVectors[i] = v; }

	unsigned int GetSegmentCount() const { return intersectionPoints.size() - 1; }
	Eigen::Vector3d GetIntersectionPoint(const unsigned int& i) const { return intersectionPoints[i]; }
	Eigen::Vector3d GetControlVector(const unsigned int& i) const { return controlVectors[i]; }

	const Vector3DVectors& GetIntersectionPoints() const { return intersectionPoints; }
	const Vector3DVectors& GetControlVectors() const { return controlVectors; }

private:
	Vector3DVectors intersectionPoints;
	Vector3DVectors controlVectors;
};

Vector3DVectors ComputeSpline(const Spline& s, const unsigned int& segmentResolution)
{
	const auto segments(s.GetSegmentCount());
	Vector3DVectors points(segments * segmentResolution);
	for (unsigned int i = 0; i < segments; ++i)
	{
		double t(0.0);
		const double tStep(1.0 / segmentResolution);
		for (unsigned int j = 0; j < segmentResolution; ++j)
		{
			const Eigen::Vector3d p0(s.GetIntersectionPoint(i));
			const Eigen::Vector3d p1(s.GetIntersectionPoint(i) - s.GetControlVector(i));
			const Eigen::Vector3d p2(s.GetIntersectionPoint(i + 1) + s.GetControlVector(i + 1));
			const Eigen::Vector3d p3(s.GetIntersectionPoint(i + 1));

			points[i * segmentResolution + j] = pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 + 3.0 * (1. - t) * t * t * p2 + pow(t, 3) * p3;
			t += tStep;
		}
	}

	return points;
}

double ComputeError(const Spline& s, const Vector3DVectors& goalPoints)
{
	const unsigned int resolution(1000);
	const auto sPoints(ComputeSpline(s, resolution));
	double e(0.0);
	for (const auto& p : sPoints)
	{
		double minDistance(std::numeric_limits<double>::max());
		for (const auto& v : goalPoints)
		{
			const auto distance((p - v).norm());
			if (distance < minDistance)
				minDistance = distance;
		}
		e += minDistance;
	}

	return e;
}

Vector3DVectors BuildControlVectors(Eigen::VectorXd x, const Eigen::VectorXd* initialGuess = nullptr)
{
	Vector3DVectors controlVectors;
	controlVectors.push_back(Eigen::Vector3d(0.0, -fabs(x(0)), 0.0));

	if (initialGuess)
	{
		for (int i = 1; i < x.size() - 1; ++i)
		{
			if (x(i) * (*initialGuess)(i) < 0.0)
				x(i) *= -1.0;
		}
	}

	for (int i = 1; i < x.size() - 1; i += 3)
		controlVectors.push_back(Eigen::Vector3d(x(i), x(i + 1), x(i + 2)));

	controlVectors.push_back(Eigen::Vector3d(0.0, fabs(x(x.size() - 1)), 0.0));

	return controlVectors;
}

struct SplineFitArgs : public Optimizer::AdditionalArgs
{
	SplineFitArgs(const Vector3DVectors& goalPoints,
		const Vector3DVectors& intersectionPoints, const Eigen::VectorXd& initialGuess)
		: goalPoints(goalPoints), intersectionPoints(intersectionPoints), initialGuess(initialGuess) {}

	const Vector3DVectors& goalPoints;
	const Vector3DVectors& intersectionPoints;
	const Eigen::VectorXd initialGuess;
};

Eigen::VectorXd DoIteration(const Eigen::VectorXd& guess, const Optimizer::AdditionalArgs* args)
{
	const auto& arguments(*dynamic_cast<const SplineFitArgs*>(args));
	const auto controlVectors(BuildControlVectors(guess, &arguments.initialGuess));
	Spline s;
	for (unsigned int i = 0; i < controlVectors.size(); ++i)
		s.AddPoint(arguments.intersectionPoints[i], controlVectors[i]);

	return Eigen::VectorXd(guess.size()).setOnes() * ComputeError(s, arguments.goalPoints);// TODO:  Is this correct?  Can we return 1x1?
}

void FitSplineToPoints(const Vector3DVectors& points, Spline& spline)
{
	constexpr unsigned int splineSegmentCount(3);// Assume that we'll get a good fit if we choose three segments.
	assert(points.size() > splineSegmentCount);
	Eigen::VectorXd initialGuess((splineSegmentCount - 1) * 3 + 2, 1);
	initialGuess.setOnes();

	spline.AddPoint(points.front(), Eigen::Vector3d(0.0, -1.0, 0.0));

	const unsigned int i1(points.size() / splineSegmentCount);
	for (unsigned int a = 1; a < splineSegmentCount; ++a)
	{
		const double signAdjust(1.0);
		spline.AddPoint(points[i1 * a], (points[i1 * a - 1] - points[i1 * a + 1]) * signAdjust);
		initialGuess((a - 1) * 3 + 1) = spline.GetControlVector(a)(0);
		initialGuess((a - 1) * 3 + 2) = spline.GetControlVector(a)(1);
		initialGuess((a - 1) * 3 + 3) = spline.GetControlVector(a)(2);
	}

	spline.AddPoint(points.back(), Eigen::Vector3d(0.0, 1.0, 0.0));

	/*std::cout << "\nIntersection Points:\n";
	for (const auto& ip : spline.GetIntersectionPoints())
		std::cout << ip.transpose() << '\n';

	std::cout << "\nInitial Control Vectors:\n";
	for (const auto& cp : spline.GetControlVectors())
		std::cout << cp.transpose() << '\n';*/

	SplineFitArgs arguments(points, spline.GetIntersectionPoints(), initialGuess);
	const unsigned int iterationLimit(10000);
	NelderMead<(splineSegmentCount - 1) * 3 + 2> optimizer(DoIteration, iterationLimit, &arguments);
	optimizer.SetInitialGuess(initialGuess);
	const auto x(optimizer.Optimize());
	const auto newControlVectors(BuildControlVectors(x));

	for (unsigned int i = 0; i < newControlVectors.size(); ++i)
		spline.SetControlVector(i, newControlVectors[i]);

	/*std::cout << "\nFinal Control Vectors:\n";
	for (const auto& cp : spline.GetControlVectors())
		std::cout << cp.transpose() << '\n';*/
}

double ComputeLength(const Vector3DVectors& p)
{
	double length(0.0);
	for (unsigned int i = 1; i < p.size(); ++i)
		length += (p[i] - p[i - 1]).norm();
	return length;
}

bool FindIntersectionOfTwoCircles(const Eigen::Vector2d& c1, const double& r1,
	const Eigen::Vector2d& c2, const double& r2, Eigen::Vector2d& isect1, Eigen::Vector2d& isect2)
{
	const double distance((c1 - c2).norm());
	if (distance > r1 + r2 /*|| distance < fabs(r1 - r2)*/ || (distance == 0.0 && r1 == r2))// If there are no solutions, or infinite solutions, we cannot proceed
		return false;

	const double a((r1 * r1 - r2 * r2 + distance * distance) / (2.0 * distance));
	const double h(sqrt(r1 * r1 - a * a));
	const Eigen::Vector2d p(c1 + a * (c2 - c1) / distance);

	isect1(0) = p(0) + h * (c2(1) - c1(1)) / distance;
	isect1(1) = p(1) - h * (c2(0) - c1(0)) / distance;

	isect2(0) = p(0) - h * (c2(1) - c1(1)) / distance;
	isect2(1) = p(1) + h * (c2(0) - c1(0)) / distance;

	return true;
}

Eigen::Vector2d ChooseBestIntersection(const Eigen::Vector2d& isect1, const Eigen::Vector2d& isect2, const Vector2DVectors& c)
{
	// TODO:  Improve this.  Should also consider distance between intersection results as criteria?  And/or distance from previous point?
	/*if (c.size() < 2)
		return isect1;

	if ((c.back() - isect1).norm() > (c.back() - isect2).norm())
		return isect1;*/
	return isect2;
}

bool GenerateFlatPattern(const Spline& s1, const Spline& s2, const double& stepTarget, Vector2DVectors& flatPatternPoints, const unsigned int& targetOutputPointCount)
{
	const unsigned int resolution(1000);
	auto c1(ComputeSpline(s1, resolution));
	auto c2(ComputeSpline(s2, resolution));

	double s1Length(ComputeLength(c1));
	double s2Length(ComputeLength(c2));

	// Recalculate with resolution fine enough to give good distance resolution
	const double factor(100.0);
	c1 = ComputeSpline(s1, static_cast<unsigned int>(s1Length / stepTarget * factor));
	c2 = ComputeSpline(s2, static_cast<unsigned int>(s2Length / stepTarget * factor));

	s1Length = ComputeLength(c1);
	s2Length = ComputeLength(c2);

	const double step1(s1Length > s2Length ? stepTarget : stepTarget * s1Length / s2Length);
	const double step2(s2Length > s1Length ? stepTarget : stepTarget * s2Length / s1Length);

	Vector2DVectors curve1, curve2;
	double d((c1.front() - c2.front()).norm());
	curve1.push_back((Eigen::Vector2d() << 0.0, 0.0).finished());
	curve2.push_back((Eigen::Vector2d() << d, 0.0).finished());

	unsigned int i1(1), i2(1);
	unsigned int i1Last(0), i2Last(0);
	while (i1 < c1.size() - 1 && i2 < c2.size() - 1)
	{
		for (; i1 < c1.size() - 1; ++i1)
		{
			if ((c1[i1] - c1[i1Last]).norm() > step1)
				break;
		}

		for (; i2 < c2.size() - 1; ++i2)
		{
			if ((c2[i2] - c2[i2Last]).norm() > step2)
				break;
		}

		const double d1From1((c1[i1] - c1[i1Last]).norm());
		const double d1From2((c1[i1] - c2[i2Last]).norm());
		const double d2From1((c2[i2] - c1[i1Last]).norm());
		const double d2From2((c2[i2] - c2[i2Last]).norm());
		i1Last = i1;
		i2Last = i2;

		Eigen::Vector2d isect11, isect12, isect21, isect22;
		if (!FindIntersectionOfTwoCircles(curve1.back(), d1From1, curve2.back(), d1From2, isect11, isect12))
			return false;
		if (!FindIntersectionOfTwoCircles(curve1.back(), d2From1, curve2.back(), d2From2, isect21, isect22))
			return false;

		curve1.push_back(ChooseBestIntersection(isect11, isect12, curve1));
		curve2.push_back(ChooseBestIntersection(isect21, isect22, curve2));
	}

	auto decimate([](Vector2DVectors& curve, const unsigned int& increment)
	{
		unsigned int swapCount(0);
		for (unsigned int i = 0; i < curve.size(); ++i)
		{
			if (i % increment == 0 || i == curve.size() - 1)
				continue;
			auto it(curve.begin() + i - swapCount);
			std::rotate(it, it + 1, curve.end());
			++swapCount;
		}
		curve.erase(curve.begin() + curve.size() - swapCount, curve.end());
	});

	const unsigned int increment(std::max(static_cast<unsigned int>(curve1.size() / targetOutputPointCount), 1U));
	decimate(curve1, increment);
	decimate(curve2, increment);

	flatPatternPoints = curve1;
	flatPatternPoints.insert(flatPatternPoints.end(), curve2.rbegin(), curve2.rend());

	return true;
}

int main(int argc, char* argv[])
{
	if (argc != 2)
	{
		std::cout << "Usage:  " << argv[0] << " <input file>\n"
			<< "  Input file must be comma-delimited and must contain four columns.\n"
			<< "  The first three columns are (x,y,z) for a series of points\n"
			<< "  describing one spline, and columns 4-5 are (x,y,z) for a series of\n"
			<< "  points describing the second spline.  Points should only be included\n"
			<< "  for half of each curve (i.e. positive y-ordinates only).  It is\n"
			<< "  assumed that x-z plane symmetry is desired, and curves are\n"
			<< "  constrained to have slopes parallel to the y-axis where the curves\n"
			<< "  meet the x-z plane.\n" << std::endl;
	}

	Vector3DVectors curve1, curve2;
	if (!ReadInputFile(argv[1], curve1, curve2))
		return 1;

	Spline spline1, spline2;
	FitSplineToPoints(curve1, spline1);
	FitSplineToPoints(curve2, spline2);

	const unsigned int res(30);
	const auto c1(ComputeSpline(spline1, res));
	const auto c2(ComputeSpline(spline2, res));
	std::ofstream splinesOut("splinesOut.csv");
	for (unsigned int i = 0; i < c1.size(); ++i)
		splinesOut << c1[i](0) << ',' << c1[i](1) << ',' << c1[i](2) << ',' << c2[i](0) << ',' << c2[i](1) << ',' << c2[i](2) << '\n';//*/

	const double distanceResolution(0.01);
	const unsigned int targetOutputPointCount(100);
	Vector2DVectors flatPatternPoints;
	if (!GenerateFlatPattern(spline1, spline2, distanceResolution, flatPatternPoints, targetOutputPointCount))
		return 1;

	std::ofstream flatPatternOut("flatPattern.csv");
	flatPatternOut.precision(10);
	for (unsigned int i = 0; i < flatPatternPoints.size(); ++i)
		flatPatternOut << std::fixed << flatPatternPoints[i](0) << ',' << flatPatternPoints[i](1) << '\n';

	return 0;
}


#pragma once

#include <Eigen/Dense>

template <typename T>
void deg2rad(T &deg)
{
    deg *= M_PI / 180.;
}

template <typename T>
Eigen::Matrix<T, 4, 1> eulerToQuaternion(const Eigen::Matrix<T, 3, 1> &rpy)
{
    Eigen::Quaternion<T> q = Eigen::AngleAxis<T>(rpy.x(), Eigen::Matrix<T, 3, 1>::UnitX()) *
                             Eigen::AngleAxis<T>(rpy.y(), Eigen::Matrix<T, 3, 1>::UnitY()) *
                             Eigen::AngleAxis<T>(rpy.z(), Eigen::Matrix<T, 3, 1>::UnitZ());
    Eigen::Matrix<T, 4, 1> quat;
    quat << q.w(), q.vec();

    return quat;
}

template <typename T>
Eigen::Matrix<T, 4, 4> omegaMatrix(const Eigen::Matrix<T, 3, 1> &w)
{
    Eigen::Matrix<T, 4, 4> omegaMatrix;
    omegaMatrix << T(0.), -w(0), -w(1), -w(2),
        w(0), T(0.), w(2), -w(1),
        w(1), -w(2), T(0.), w(0),
        w(2), w(1), -w(0), T(0.);

    return omegaMatrix;
}

template <typename T>
Eigen::Matrix<T, 3, 3> omegaMatrixReduced(const Eigen::Matrix<T, 3, 1> &q)
{
    Eigen::Matrix<T, 3, 3> omegaMatrix;
    const T qw = sqrt(1. - q.squaredNorm());
    omegaMatrix << qw, -q(2), q(1),
        q(2), qw, -q(0),
        -q(1), q(0), qw;

    return omegaMatrix;
}

template <typename T>
Eigen::Matrix<T, 3, 3> EulerRotationMatrix(const Eigen::Matrix<T, 3, 1> &eta)
{
    const T phi = eta(0);
    const T theta = eta(1);
    const T psi = eta(2);

    Eigen::Matrix<T, 3, 3> R;
    R.row(0) << cos(theta) * cos(psi), -cos(theta) * sin(psi), sin(theta);
    R.row(1) << sin(phi) * sin(theta) * cos(psi) + cos(phi) * sin(psi),
        cos(phi) * cos(psi) - sin(phi) * sin(theta) * sin(psi), -sin(phi) * cos(theta);
    R.row(2) << -cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi),
        sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi), cos(phi) * cos(theta);

    return R;
}

template <typename T>
Eigen::Matrix<T, 3, 3> EulerRotationJacobian(const Eigen::Matrix<T, 3, 1> &eta)
{
    const T phi = eta(0);
    const T theta = eta(1);
    const T psi = eta(2);

    Eigen::Matrix<T, 3, 3> J;
    J.row(0) << cos(psi), -sin(psi), 0.;
    J.row(1) << cos(theta) * sin(psi), cos(theta) * cos(psi), 0;
    J.row(2) << -sin(theta) * cos(psi), sin(theta) * sin(psi), cos(theta);

    return 1. / cos(theta) * J;
}


#include <vector>
#include <iostream>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>
#include <boost/gil/extension/io/png.hpp>

#include <boost/gil/extension/io/jpeg.hpp>
using namespace boost::gil;
using namespace std;
int main()
{
    //gray8_image_t img;
    //read_image("test_adaptive.png", img, png_tag{});
    //gray8_image_t img_out(img.dimensions());

    gray8_image_t img;
    read_image("src_view.png", img, png_tag{});
    gray8_image_t img_out(img.dimensions()), img_out1(img.dimensions());

    std::vector<float> v(9, 1.0f / 9.0f);
    detail::kernel_2d<float> kernel(v.begin(), v.size(), 1, 1);
    detail::convolve_2d(view(img), kernel, view(img_out1));

    //write_view("out-convolve2d.png", view(img_out), png_tag{});
    write_view("out-convolve2d.png", view(img_out1), jpeg_tag{});

    //------------------------------------//
    std::vector<float> v1(3, 1.0f / 3.0f);
    kernel_1d<float> kernel1(v1.begin(), v1.size(), 1);

    detail::convolve_1d<gray32f_pixel_t>(const_view(img), kernel1, view(img_out), boundary_option::extend_zero);
    write_view("out-convolve_option_extend_zero.png", view(img_out), png_tag{});

    if (equal_pixels(view(img_out1), view(img_out)))cout << "convolve_option_extend_zero" << endl;

    cout << "done\n";
    cin.get();

    return 0;
}


// Eigen::Ref can be used to refer to a matrix *or* a block without copying.
// The referred matrix must have its memory fully allocated: cannot be resized.

#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>

#include <Eigen/Dense>

using namespace std;

Eigen::MatrixXd A = Eigen::MatrixXd::Random(1, 2);
Eigen::Ref<Eigen::MatrixXd> ref_from_address(Eigen::MatrixXd *address) {
  return *address;
}

int main() {
  std::cout << "A: " << std::endl << A << std::endl << std::endl;

  Eigen::MatrixXd *A_address = &A;
  std::cout << "A_address: " << A_address << std::endl;

  std::cout << "A = Eigen::MatrixXd::Random(2, 4);" << std::endl;
  A = Eigen::MatrixXd::Random(2, 4);
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address) =  Eigen::MatrixXd::Random(2, 4);"
            << " // Okay, since shape matches" << std::endl;
  ref_from_address(A_address) =  Eigen::MatrixXd::Random(2, 4);
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address).col(2) = "
            << "Eigen::MatrixXd::Zero(2, 1); // Also okay" << std::endl;
  ref_from_address(A_address).col(2) =  Eigen::MatrixXd::Zero(2, 1);
  std::cout << A << std::endl << std::endl;

  std::cout << "// ref_from_address(A_address) =  "
            << "Eigen::MatrixXd::Ones(2, 10); // ERROR! "
            << "\"DenseBase::resize() does not actually allow to resize.\""
            << std::endl << std::endl;
  // ref_from_address(A_address) =  Eigen::MatrixXd::Ones(2, 10);

  std::cout << "// First need to directly resize NOT USING REF. This destroys "
            << "coeffs if the number of coeffs is different." << std::endl;
  std::cout << "A_address->resize(2, 10);  " << std::endl;
  A_address->resize(2, 10);
  // ref_from_address(A_address).resize(2, 4);  // This gives error.
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address) =  "
            << "Eigen::MatrixXd::Ones(2, 10); // Now okay" << std::endl;
  ref_from_address(A_address) =  Eigen::MatrixXd::Ones(2, 10);
  std::cout << A << std::endl << std::endl;

  std::cout << "ref_from_address(A_address) << 1, 3, 5, 7, 9, 11, 13, 15, 17, "
            << "19, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20; // Also okay"
            << std::endl;
  ref_from_address(A_address) <<
      1, 3, 5, 7, 9, 11, 13, 15, 17, 19, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20;
  std::cout << A << std::endl << std::endl;

  std::cout << "// Aside: resizing is conservative when the number of coeffs "
            << "remains the same." << std::endl;
  std::cout << "A_address->resize(2, 10);  " << std::endl;
  A_address->resize(4, 5);
  std::cout << A << std::endl << std::endl;

  std::cout << "// Coefficient access" << std::endl;
  std::cout << "ref_from_address(A_address)(1, 2) = "
            << ref_from_address(A_address)(1, 2) << std::endl;
  std::cout << "ref_from_address(A_address).data()[9] = "
            << ref_from_address(A_address).data()[9] << std::endl;

  return 0;
}


#pragma once

#include <math/Vec.hpp>
#include <boost/range.hpp>

template <class T>
struct Rectangle {
	Rectangle() : top_(0), left_(0), bottom_(0), right_(0) { }

	Rectangle(T const & top, T const & left,
		T const & bottom, T const & right)
	:
		top_(top), left_(left),
		bottom_(bottom), right_(right)
	{ }

	T const & get_top() const { return top_; }
	void set_top(T const & top) { top_ = top; }

	T const & get_left() const { return left_; }
	void set_left(T const & left) { left_ = left; }

	T const & get_bottom() const { return bottom_; }
	void set_bottom() { return bottom_; }

	T const & get_right() const { return right_; }
	void set_right(T const & right) { right_ = right; }

	template <class U>
	bool contains(U const & x, U const & y) const {
		return left_ <= x && x < right_
			&& bottom_ <= y && y < top_;
	}

	template <class U>
	bool contains(Vec<2, U> const & v) const {
		return contains(v.x(), v.y());
	}

	template <class stream>
	friend stream & operator << (stream & out, Rectangle<T> const & rect) {
		return out << "Rect("
			<< rect.top_ << ", "
			<< rect.left_ << ", "
			<< rect.bottom_ << ", "
			<< rect.right_ << ")";
	}

	private:
		T top_, left_, bottom_, right_;
};

template <class Iterator>
inline Rectangle<float> get_bounding_rectangle(Iterator begin, Iterator end) {
	Vec2 lower_left = *begin, upper_right = *begin;
	++begin;

	for (; begin != end; ++begin) {
		lower_left = min(lower_left, *begin);
		upper_right = max(upper_right, *begin);
	}

	return Rectangle<float>(upper_right.y(), lower_left.x(),
		lower_left.y(), upper_right.x());
}

template <class Container>
inline Rectangle<float> get_bounding_rectangle(Container const & c) {
	return get_bounding_rectangle(boost::begin(c), boost::end(c));
}

template <class Iterator, class F>
inline Rectangle<float> get_bounding_rectangle(Iterator begin, Iterator end, F const & f) {
	Vec2 lower_left = f(*begin), upper_right = f(*begin);
	++begin;

	for (; begin != end; ++begin) {
		lower_left = min(lower_left, f(*begin));
		upper_right = max(upper_right, f(*begin));
	}

	return Rectangle<float>(upper_right.y(), lower_left.x(),
		lower_left.y(), upper_right.x());
}


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

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

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

#include <geometry_test_common.hpp>

#include <boost/geometry/strategies/cartesian/side_of_intersection.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>

namespace bg = boost::geometry;

int test_main(int, char* [])
{
    typedef bg::model::d2::point_xy<int> point;
    typedef bg::model::segment<point> segment;

    segment a(point(20, 10), point(10, 20));

    segment b1(point(11, 16), point(20, 14));  // IP with a: (14.857, 15.143)
    segment b2(point(10, 16), point(20, 14));  // IP with a: (15, 15)

    segment c1(point(15, 16), point(13, 8));
    segment c2(point(15, 16), point(14, 8));
    segment c3(point(15, 16), point(15, 8));

    typedef bg::strategy::side::side_of_intersection side;

    BOOST_CHECK_EQUAL( 1, side::apply(a, b1, c1));
    BOOST_CHECK_EQUAL(-1, side::apply(a, b1, c2));
    BOOST_CHECK_EQUAL(-1, side::apply(a, b1, c3));

    BOOST_CHECK_EQUAL( 1, side::apply(a, b2, c1));
    BOOST_CHECK_EQUAL( 1, side::apply(a, b2, c2));
    BOOST_CHECK_EQUAL( 0, side::apply(a, b2, c3));

    // Check internal calculation-method:
    BOOST_CHECK_EQUAL(-1400, side::side_value<int>(a, b1, c2));
    BOOST_CHECK_EQUAL( 2800, side::side_value<int>(a, b1, c1));

    BOOST_CHECK_EQUAL (2800, side::side_value<int>(a, b1, c1));
    BOOST_CHECK_EQUAL(-1400, side::side_value<int>(a, b1, c2));
    BOOST_CHECK_EQUAL(-5600, side::side_value<int>(a, b1, c3));

    BOOST_CHECK_EQUAL(12800, side::side_value<int>(a, b2, c1));
    BOOST_CHECK_EQUAL( 6400, side::side_value<int>(a, b2, c2));
    BOOST_CHECK_EQUAL(    0, side::side_value<int>(a, b2, c3));

    // TODO: we might add a check calculating the IP, determining the side
    // with the normal side strategy, and verify the results are equal

    return 0;
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/matrix/infinity_norm.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(infinity_norm)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		int,
		3,
		3
	>
	matrix_type;

	matrix_type const t(
		fcppt::math::matrix::row(
			3, 5, 7
		),
		fcppt::math::matrix::row(
			2, 6, 4
		),
		fcppt::math::matrix::row(
			0, 2, 8
		)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::infinity_norm(
			t
		),
		15
	);
}


/*
 * Copyright 2016 Maikel Nadolski
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "hidden-markov-models.t.h"

#include "maikel/hmm/stochastical_conditions.h"

#include <fstream>
#include <Eigen/Dense>

namespace {

CASE( "Read array data in 'array.dat' and its same as hard coded." )
{
  std::ifstream datafile("array.dat");
  EXPECT(datafile);
  Eigen::ArrayXf array_hardcoded(3);
  array_hardcoded << 0.3, 0.3, 0.4;
  Eigen::ArrayXf array_from_file(3);
  std::copy(std::istream_iterator<float>(datafile),
            std::istream_iterator<float>(),
            array_from_file.data());
  EXPECT(array_hardcoded.size() == array_from_file.size());
  for (int i = 0; i < array_from_file.size(); ++i)
    EXPECT(array_from_file(i) == array_hardcoded(i));
}

CASE( "is stochastic or not" )
{
  Eigen::ArrayXf array(3);
  array << 0.5, 0.2, 0.3;
  EXPECT(maikel::hmm::is_probability_array(array));
}

CASE( "is stochastic matrix ")
{
  Eigen::MatrixXf matrix(2,2);
  matrix << 0.3, 0.7,
            0.5, 0.5;
  EXPECT(maikel::hmm::rows_are_probability_arrays(matrix));
}

}


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

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2014 Adam Wulkiewicz, Lodz, Poland.

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

//[boost_polygon_point
//`Shows how to use Boost.Polygon point_data within Boost.Geometry

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_polygon.hpp>

int main()
{
    boost::polygon::point_data<int> a(1, 2), b(3, 4);
    std::cout << "Distance (using Boost.Geometry): "
        << boost::geometry::distance(a, b) << std::endl;
    std::cout << "Distance (using Boost.Polygon): "
        << boost::polygon::euclidean_distance(a, b) << std::endl;

    return 0;
}

//]

//[boost_polygon_point_output
/*`
Output:
[pre
Distance (using Boost.Geometry): 2.82843
Distance (using Boost.Polygon): 2.82843
]
*/
//]


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

#ifndef BOOST_MATH_EXPM1_INCLUDED
#define BOOST_MATH_EXPM1_INCLUDED

#include <cmath>
#include <math.h> // platform's ::expm1
#include <boost/limits.hpp>
#include <boost/math/special_functions/detail/series.hpp>

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::exp; using ::fabs; }
#endif

namespace boost{ namespace math{

namespace detail{
//
// Functor expm1_series returns the next term in the Taylor series
// x^k / k!
// each time that operator() is invoked.
//
template <class T>
struct expm1_series
{
   typedef T result_type;

   expm1_series(T x)
      : k(0), m_x(x), m_term(1) {}

   T operator()()
   {
      ++k;
      m_term *= m_x;
      m_term /= k;
      return m_term;
   }

   int count()const
   {
      return k;
   }

private:
   int k;
   const T m_x;
   T m_term;
   expm1_series(const expm1_series&);
   expm1_series& operator=(const expm1_series&);
};

} // namespace

//
// Algorithm expm1 is part of C99, but is not yet provided by many compilers.
//
// This version uses a Taylor series expansion for 0.5 > |x| > epsilon.
//
template <class T>
T expm1(T x)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif

   T a = std::fabs(x);
   if(a > T(0.5L))
      return std::exp(x) - T(1);
   if(a < std::numeric_limits<T>::epsilon())
      return x;
   detail::expm1_series<T> s(x);
   T result = detail::kahan_sum_series(s, std::numeric_limits<T>::digits + 2);
   return result;
}
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x564))
inline float expm1(float z)
{
   return expm1<float>(z);
}
inline double expm1(double z)
{
   return expm1<double>(z);
}
inline long double expm1(long double z)
{
   return expm1<long double>(z);
}
#endif

#ifdef expm1
#  ifndef BOOST_HAS_expm1
#     define BOOST_HAS_expm1
#  endif
#  undef expm1
#endif

#ifdef BOOST_HAS_EXPM1
#  if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)
inline float expm1(float x){ return ::expm1f(x); }
inline long double expm1(long double x){ return ::expm1l(x); }
#else
inline float expm1(float x){ return ::expm1(x); }
#endif
inline double expm1(double x){ return ::expm1(x); }
#endif

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


#ifndef MATHTOOLBOX_CLASSICAL_MDS_HPP
#define MATHTOOLBOX_CLASSICAL_MDS_HPP

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

namespace mathtoolbox
{
    /// \brief This function computes low-dimensional embedding by using classical multi-dimensional scaling (MDS)
    /// \param D Distance (dissimilarity) matrix and a target dimension for embedding
    /// \param dim Target dimension
    /// \return Coordinate matrix whose i-th column corresponds to the embedded coordinates of the i-th entry
    Eigen::MatrixXd ComputeClassicalMds(const Eigen::MatrixXd& D, unsigned dim);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_CLASSICAL_MDS_HPP


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 */

#define BOOST_TEST_MODULE EuclideanPlane3D
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#include <stdexcept>

#include "EuclideanPlane3D.h"
#include "Error.h"

namespace utf = boost::unit_test;
namespace euc = cupcfd::geometry::euclidean;

// === Constructor ===
// Test 1: Test the correct values are setup by the constructor
BOOST_AUTO_TEST_CASE(constructor_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	// Check Points have been copied
	BOOST_CHECK_EQUAL(plane.p1.cmp[0], p1.cmp[0]);
	BOOST_CHECK_EQUAL(plane.p1.cmp[1], p1.cmp[1]);
	BOOST_CHECK_EQUAL(plane.p1.cmp[2], p1.cmp[2]);

	BOOST_CHECK_EQUAL(plane.p2.cmp[0], p2.cmp[0]);
	BOOST_CHECK_EQUAL(plane.p2.cmp[1], p2.cmp[1]);
	BOOST_CHECK_EQUAL(plane.p2.cmp[2], p2.cmp[2]);

	BOOST_CHECK_EQUAL(plane.p3.cmp[0], p3.cmp[0]);
	BOOST_CHECK_EQUAL(plane.p3.cmp[1], p3.cmp[1]);
	BOOST_CHECK_EQUAL(plane.p3.cmp[2], p3.cmp[2]);

	// Check Plane Equation (These are the Non GCD Versions)
	BOOST_TEST(plane.a == 432.25);
	BOOST_TEST(plane.b == -228.15);
	BOOST_TEST(plane.c == -130.0999);
	BOOST_TEST(plane.d == 340.35);
}

// === operator = ===
// Test 1: Check copy works successfully
BOOST_AUTO_TEST_CASE(operator_single_equals_test1, * utf::tolerance(0.00001))
{

}

// === operator == ===
// Test 1: Test that true is obtained when the stored values are the same
BOOST_AUTO_TEST_CASE(operator_double_equals_test1, * utf::tolerance(0.00001))
{

}

// Test 2: Test that false is obtained when p1 is different
BOOST_AUTO_TEST_CASE(operator_double_equals_test2, * utf::tolerance(0.00001))
{

}

// Test 3: Test that false is obtained when p2 is different
BOOST_AUTO_TEST_CASE(operator_double_equals_test3, * utf::tolerance(0.00001))
{

}

// Test 4: Test that false is obtained when p3 is different
BOOST_AUTO_TEST_CASE(operator_double_equals_test4, * utf::tolerance(0.00001))
{

}

// === operator != ===
// Test 1: Test that false is obtained when the stored values are the same
BOOST_AUTO_TEST_CASE(operator_double_notequals_test1, * utf::tolerance(0.00001))
{

}

// Test 2: Test that true is obtained when p1 is different and p2, p3 are the same
BOOST_AUTO_TEST_CASE(operator_double_notequals_test2, * utf::tolerance(0.00001))
{

}

// Test 3: Test that true is obtained when p2 is different and p1, p3 are the same
BOOST_AUTO_TEST_CASE(operator_double_notequals_test3, * utf::tolerance(0.00001))
{

}

// Test 4: Test that true is obtained when p3 is different and p1, p2 are the same
BOOST_AUTO_TEST_CASE(operator_double_notequals_test4, * utf::tolerance(0.00001))
{

}

// === operator < ===
// ToDo
BOOST_AUTO_TEST_CASE(operator_less_than_test1, * utf::tolerance(0.00001))
{

}

// === operator <= ===
// ToDo
BOOST_AUTO_TEST_CASE(operator_less_than_or_equals_test1, * utf::tolerance(0.00001))
{

}

// === operator > ===
// ToDo
BOOST_AUTO_TEST_CASE(operator_greater_than_test1, * utf::tolerance(0.00001))
{

}

// === operator >= ===
// ToDo
BOOST_AUTO_TEST_CASE(operator_greater_than_or_equals_test1, * utf::tolerance(0.00001))
{

}

// === getNormal ===
// Test 1: Get the correct normal from a plane object
// ToDo
BOOST_AUTO_TEST_CASE(getNormal_test1, * utf::tolerance(0.00001))
{

}

// === normal ===
// Test 1: Test correct normal
BOOST_AUTO_TEST_CASE(normal_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanVector3D<double> norm;

	norm = euc::EuclideanPlane3D<double>::calculateNormal(p1, p2, p3);

	// Wolfram Alpha gives these, but these are the GCD version
	//BOOST_TEST(norm.x == 0.854612);
	//BOOST_TEST(norm.y == -0.451081);
	//BOOST_TEST(norm.z == -0.257224);

	BOOST_TEST(norm.cmp[0] == 432.25);
	BOOST_TEST(norm.cmp[1] == -228.15);
	BOOST_TEST(norm.cmp[2] == -130.0999);
}

// === computeScalarPlaneEquation ===
// Test 1: Test correct equation is calculated
BOOST_AUTO_TEST_CASE(computeScalarPlaneEquation_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);
	double a, b, c, d;

	plane.computeScalarPlaneEquation(&a, &b, &c, &d);

	// Wolfram Alpha gives these, but these are the GCD version - factor ~ -131
	//BOOST_TEST(a == -3.32244);
	//BOOST_TEST(b == 1.75365);
	//BOOST_TEST(c == 1.0);
	//BOOST_TEST(d == -2.61606);

	BOOST_TEST(a == 432.25);
	BOOST_TEST(b == -228.15);
	BOOST_TEST(c == -130.0999);
	BOOST_TEST(d == 340.35);
}

// === isPointOnPlane ===
// Test 1: Test true is returned when the point lies on the plane - Test point p1
BOOST_AUTO_TEST_CASE(isPointOnPlane_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	bool check = plane.isPointOnPlane(p1);
	BOOST_CHECK_EQUAL(check, true);
}

// Test 2: Test true is returned when the point lies on the plane - Test point p2
BOOST_AUTO_TEST_CASE(isPointOnPlane_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	bool check = plane.isPointOnPlane(p2);
	BOOST_CHECK_EQUAL(check, true);
}

// Test 3: Test true is returned when the point lies on the plane - Test point p3
BOOST_AUTO_TEST_CASE(isPointOnPlane_test3, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	bool check = plane.isPointOnPlane(p3);
	BOOST_CHECK_EQUAL(check, true);
}

// Test 4: Test true is returned when the point lies on the plane - Arbitrary point
BOOST_AUTO_TEST_CASE(isPointOnPlane_test4, * utf::tolerance(0.00001))
{

}

// Test 5: Test false is returned when the point does not lie on the plane
BOOST_AUTO_TEST_CASE(isPointOnPlane_test5, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPoint<double,3> p4(-2000.0, -2000.0, -2000.0);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	bool check = plane.isPointOnPlane(p4);
	BOOST_CHECK_EQUAL(check, false);
}

// === computeProjectedPoint ===
// Test 1: Test the correct projection of a point that lies above a flat plane in the z-axis
BOOST_AUTO_TEST_CASE(computeProjectedPoint_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.8, 4.0, 2.0);
	euc::EuclideanPoint<double,3> p2(21.7, 6.0, 2.0);
	euc::EuclideanPoint<double,3> p3(14.3, 1.0, 2.0);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);
	euc::EuclideanPoint<double,3> testPoint(21.7, 6.0, 21.3);

	euc::EuclideanPoint<double,3> compare(21.7, 6.0, 2.0);
	euc::EuclideanPoint<double,3> result;

	result = plane.computeProjectedPoint(testPoint);

	BOOST_TEST(compare.cmp[0] == result.cmp[0]);
	BOOST_TEST(compare.cmp[1] == result.cmp[1]);
	BOOST_TEST(compare.cmp[2] == result.cmp[2]);
}

// Test 2: Test the correct projection of a point that lies below a flat plane in the z-axis
BOOST_AUTO_TEST_CASE(computeProjectedPoint_test2, * utf::tolerance(0.00001))
{

}

// Test 3: Test the correct projection of a point that lies on a flat plane in the z-axis (i.e. should be same point)
BOOST_AUTO_TEST_CASE(computeProjectedPoint_test3, * utf::tolerance(0.00001))
{

}

// === isVectorParallel ===
// Test 1: Test that a vector is correctly identified as being in parallel
BOOST_AUTO_TEST_CASE(isVectorParallel_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	// Vector from p1 to p2 should be in parallel
	euc::EuclideanVector<double,3> vec = p2 - p1;
	bool parallel = plane.isVectorParallel(vec);
	BOOST_CHECK_EQUAL(parallel, true);
}

// Test 2: Test that a vector is correctly identified as not being in parallel
BOOST_AUTO_TEST_CASE(isVectorParallel_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	// Normal vector should not be in parallel by definition
	euc::EuclideanVector<double,3> vec(432.25, -228.15, -130.0999);
	bool parallel = plane.isVectorParallel(vec);
	BOOST_CHECK_EQUAL(parallel, false);
}

// === isVectorParallelInPlane ===
// Test 1: Test true is returned when a parallel vector in the plane is detected
BOOST_AUTO_TEST_CASE(isVectorParallelInPlane_test1, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	// Generate a vector that is parallel to the plane
	euc::EuclideanVector<double,3> vec = p2 - p1;

	// Test and Check - Select a point on the plane with the parallel vector
	bool result = plane.isVectorParallelInPlane(vec, p1);
	BOOST_CHECK_EQUAL(result, true);
}

// Test 2: Test false is returned when a vector is parallel but does not lie on the plane
BOOST_AUTO_TEST_CASE(isVectorParallelInPlane_test2, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPoint<double,3> origin(0.0, 0.0, 0.0);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	// Generate a vector that is parallel to the plane
	euc::EuclideanVector<double,3> vec = p2 - p1;

	// Test and Check - Select a point on the plane with the parallel vector
	bool result = plane.isVectorParallelInPlane(vec, origin);
	BOOST_CHECK_EQUAL(result, false);
}

// Test 3: Test false is returned when a vector is not parallel
BOOST_AUTO_TEST_CASE(isVectorParallelInPlane_test3, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	// Generate a vector that is parallel to the plane
	euc::EuclideanVector<double,3> vec(0.0, 0.0, 4.0);

	// Test and Check - Select a point on the plane with the parallel vector
	bool result = plane.isVectorParallelInPlane(vec, p1);
	BOOST_CHECK_EQUAL(result, false);
}

// === linePlaneIntersection  ===
// Test 1: Find intersection of non-parallel vector with plane
BOOST_AUTO_TEST_CASE(linePlaneIntersection_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(0.0, 0.0, 0.0);
	euc::EuclideanPoint<double,3> p2(1.0, 0.0, 0.0);
	euc::EuclideanPoint<double,3> p3(0.0, 1.0, 0.0);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	euc::EuclideanVector<double,3> vec(1.0, 1.0, -1.0);
	euc::EuclideanPoint<double,3> origin(2.0, 5.0, 1.0);

	euc::EuclideanPoint<double,3> result;
	cupcfd::error::eCodes status = plane.linePlaneIntersection(vec, origin, result);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_CHECK_EQUAL(result.cmp[0], 3.0);
	BOOST_CHECK_EQUAL(result.cmp[1], 6.0);
	BOOST_CHECK_EQUAL(result.cmp[2], 0.0);
}

// Test 2: Error code when vector is parallel in plane
BOOST_AUTO_TEST_CASE(linePlaneIntersection_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	euc::EuclideanVector<double,3> parallelVec = p2-p1;
	euc::EuclideanPoint<double,3> result;

	cupcfd::error::eCodes status = plane.linePlaneIntersection(parallelVec, p1, result);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_EUC_VEC_PARALLEL);
}

// Test 3: Error code when vector is parallel but not in plane
BOOST_AUTO_TEST_CASE(linePlaneIntersection_test3, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	euc::EuclideanVector<double,3> parallelVec = p2-p1;
	euc::EuclideanPoint<double,3> l0(0.0, 0.0, 0.0);
	euc::EuclideanPoint<double,3> result;

	cupcfd::error::eCodes status = plane.linePlaneIntersection(parallelVec, l0, result);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_EUC_VEC_PARALLEL);
}

// === shortestDistance ===
// Test 1: Correctly compute the distance to a point from the plane where both are on the positive side of the origin
BOOST_AUTO_TEST_CASE(shortestDistance_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	euc::EuclideanPoint<double,3> point(15.3,8.9, 43.5);

	double distance = plane.shortestDistance(point);

	BOOST_TEST(distance == 1.45538);
}

// Test 2: Correctly compute the distance to a point from the plane where the planes are on different sides of the origin
BOOST_AUTO_TEST_CASE(shortestDistance_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	euc::EuclideanPoint<double,3> point(201.7,165.3,198.4);

	double distance = plane.shortestDistance(point);

	BOOST_TEST(distance == 47.4512);
}

// Test 3: Correctly compute the distance to a point on the plane
BOOST_AUTO_TEST_CASE(shortestDistance_test3, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,3> p1(2.0, 3.0, 4.0);
	euc::EuclideanPoint<double,3> p2(18.6, 17.4, 33.9);
	euc::EuclideanPoint<double,3> p3(15.3, 6.7, 41.7);
	euc::EuclideanPlane3D<double> plane(p1, p2, p3);

	euc::EuclideanPoint<double,3> point(18.6, 17.4, 33.9);

	double distance = plane.shortestDistance(point);

	BOOST_TEST(distance == 0.0);
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>
#include <string>

using namespace std::string_literals;

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

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"
#include "miMaS/iteration.h"

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

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

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

int
main ( int argc , char const * argv[] )
{
  std::filesystem::path p("config.init");
  if ( argc > 1 )
    { p = argv[1]; }
  auto c = config(p);
  c.name = "vhll";

  c.create_output_directory();
  std::ofstream ofconfig( c.output_dir / "config.init" );
  ofconfig << c << "\n";
  ofconfig.close();

/* ------------------------------------------------------------------------- */
  field<double,1> fh(boost::extents[c.Nv][c.Nx]);
  complex_field<double,1> hfh(boost::extents[c.Nv][c.Nx]);

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

  ublas::vector<double> v(c.Nv,0.);
  std::generate( v.begin() , v.end() , [&,k=0]() mutable {return (k++)*fh.step.dv+fh.range.v_min;} );

  ublas::vector<double> kx(c.Nx); // beware, Nx need to be odd
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  auto tb_M1 = maxwellian( 0.5*c.alpha , c.ui , 1. ) , tb_M2 = maxwellian( 0.5*c.alpha , -c.ui , 1. );
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(fh[k].begin(),fh[k].end(),hfh[k].begin());
  }
  fh.write( c.output_dir / ("init_"+c.name+".dat") );

  iteration::iteration<double> iter;
  iter.iter = 0;
  iter.current_time = 0.;
  iter.dt = 0.5*fh.step.dv;

  ublas::vector<double> uc(c.Nx,0.);
  ublas::vector<double> E (c.Nx,0.);

  std::vector<double> ee;   ee.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> iterations; iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);
  std::vector<iteration::iteration<double>> success_iterations; success_iterations.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(c.Tf/iter.dt))+1);

  // lambda to save data, any time you want, where you want
  auto save_data = [&] ( std::string && suffix ) -> void {
    suffix = c.name + suffix;

    // save iterations informations
    auto writer_iter = [] ( auto const & it ) {
      std::stringstream ss; ss << it;
      return ss.str();
    };
    c << monitoring::data( "iterations_"+suffix+".dat"         , iterations         , writer_iter );
    c << monitoring::data( "success_iterations_"+suffix+".dat" , success_iterations , writer_iter );

    // save temporel data
    auto dt_y = [&,count=0] (auto const& y) mutable {
      std::stringstream ss; ss<<times[count++]<<" "<<y;
      return ss.str();
    };
    c << monitoring::data( "ee_"+suffix+".dat"   , ee   , dt_y );
    c << monitoring::data( "Emax_"+suffix+".dat" , Emax , dt_y );
    c << monitoring::data( "H_"+suffix+".dat"    , H    , dt_y );

    // save distribution function
    for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
    fh.write( c.output_dir / ("vp_"+suffix+".dat") );
  };

  // to stop simulation at any time (and save data)
  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[38;5;202msave data...\033[0m\n";
    save_data("_SIGINT");
  });

  const double rho_c = 1.-c.alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);

  // init E with Poisson solver, init also ee, Emax, H and times
  {
    poisson<double> poisson_solver(c.Nx,fh.range.len_x());
    ublas::vector<double> rho(c.Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<c.Nx ; ++i ) { rho[i] += (1.-c.alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = std::sqrt(std::accumulate(
      E.begin() , E.end() , 0. ,
      [&] ( double partial_sum , double ei ) {
        return partial_sum + ei*ei*fh.step.dx;
      }
    ));
    ee.push_back( electric_energy );

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
    times.push_back(0.);
  }

  // initialize memory for all temporary variables
  ublas::vector<double> J(c.Nx,0.);
  fft::spectrum_ d(c.Nx);
  field<double,1> Edvf(tools::array_view<const std::size_t>(fh.shape(),2));
  ublas::vector<double> uc1(c.Nx) , uc2(c.Nx) , uc3(c.Nx) , uc4(c.Nx) , uc5(c.Nx) , uc6(c.Nx) , uc7(c.Nx),
                        E1 (c.Nx) , E2(c.Nx)  , E3 (c.Nx) , E4 (c.Nx) , E5 (c.Nx) , E6 (c.Nx) , E7 (c.Nx);
  complex_field<double,1> hfh1(boost::extents[c.Nv][c.Nx]) , hfh2(boost::extents[c.Nv][c.Nx]) ,
                          hfh3(boost::extents[c.Nv][c.Nx]) , hfh4(boost::extents[c.Nv][c.Nx]) ,
                          hfh5(boost::extents[c.Nv][c.Nx]) , hfh6(boost::extents[c.Nv][c.Nx]) ,
                          hfh7(boost::extents[c.Nv][c.Nx]) ;

  while (  iter.current_time < c.Tf ) {
    std::cout << "\r" << iteration::time(iter) << std::flush;

///////////////////////////////////////////////////////////////////////////////
// DP4(3) /////////////////////////////////////////////////////////////////////

    // STAGE 1
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc1[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c - 0.5*iter.dt*J[i]*s05/sqrt_rho_c;
        E1[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh1[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) - 0.5*iter.dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt);
        }
      }
    } // end stage 1

    // STAGE 2
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh1[k].begin(),hfh1[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E1);

      double c05 = std::cos(0.5*iter.dt*sqrt_rho_c), s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc2[i] =  uc[i]*c05 + E[i]*s05/sqrt_rho_c;
        E2[i]  = -uc[i]*s05*sqrt_rho_c + E[i]*c05 - 0.5*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh2[k][i] = hfh[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) - 0.5*iter.dt*d[i];
        }
      }
    } // end stage 2

    // STAGE 3
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh2[k].begin(),hfh2[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E2);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc3[i] =  uc[i]*c1 + E[i]*s1/sqrt_rho_c - iter.dt*J[i]*s05/sqrt_rho_c;
        E3[i]  = -uc[i]*s1*sqrt_rho_c + E[i]*c1 - iter.dt*J[i]*c05;
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh3[k][i] = hfh[k][i]*std::exp(-I*kx[i]*v[k]*iter.dt) - iter.dt*d[i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt);
        }
      }
    } // end stage 3

    // STAGE 4
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh3[k].begin(),hfh3[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E3);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc4[i] = -(1./3.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./3.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./3.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/3.;
        E4[i]  = -(1./3.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./3.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./3.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/3. - (1./6.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh4[k][i] = -(1./3.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*iter.dt) + (1./3.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + (2./3.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + hfh3[k][i]/3. - (1./6.)*iter.dt*d[i];
        }
      }
    } // end stage 4

    // STAGE 5
    {
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh4[k].begin(),hfh4[k].end(),fh[k].begin()); }
      J = fh.courant();
      Edvf = weno::trp_v(fh,E4);

      double c1  = std::cos(iter.dt*sqrt_rho_c)     , s1  = std::sin(iter.dt*sqrt_rho_c)    ,
             c05 = std::cos(0.5*iter.dt*sqrt_rho_c) , s05 = std::sin(0.5*iter.dt*sqrt_rho_c);
      for ( auto i=0 ; i<c.Nx ; ++i ) {
        uc5[i] = -(1./5.)*(  uc[i]*c1 + E[i]*s1/sqrt_rho_c ) + (1./5.)*(  uc1[i]*c05 + E1[i]*s05/sqrt_rho_c ) + (2./5.)*(  uc2[i]*c05 + E2[i]*s05/sqrt_rho_c ) + uc3[i]/5. + (2./5.)*uc4[i];
        E5[i]  = -(1./5.)*( -uc[i]*s1*sqrt_rho_c + E[i]*c1 ) + (1./5.)*( -uc1[i]*s05*sqrt_rho_c + E1[i]*c05 ) + (2./5.)*( -uc2[i]*s05*sqrt_rho_c + E2[i]*c05 ) + E3[i]/5. + (2./5.)*E4[i] - (1./10.)*iter.dt*J[i];
      }
      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) {
        d.fft(&(Edvf[k][0]));
        for ( auto i=0 ; i<c.Nx ; ++i ) {
          hfh5[k][i] = -(1./5.)*hfh[k][i]*std::exp(-I*kx[i]*v[k]*iter.dt) + (1./5.)*hfh1[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + (2./5.)*hfh2[k][i]*std::exp(-0.5*I*kx[i]*v[k]*iter.dt) + hfh3[k][i]/5. + (2./5.)*hfh4[k][i] - 0.1*iter.dt*d[i];
        }
      }
    } // end stage 5

///////////////////////////////////////////////////////////////////////////////
// MONITORING /////////////////////////////////////////////////////////////////

    // CHECH local error for compute next time step
    iter.E_error(E5,E4,fh.step.dx);
    iter.hfh_error(hfh5,hfh4,fh.step.dx*fh.step.dv);
    iter.success = std::abs(iter.error() - c.tol) <= c.tol;

    std::cout << " -- " << iteration::error(iter) << std::flush;
    if ( iter.success )
    {
      // SAVE TIME STEP
      std::copy( uc4.begin()  , uc4.end()  , uc.begin()  );
      std::copy( E4.begin()   , E4.end()   , E.begin()   );
      std::copy( hfh4.begin() , hfh4.end() , hfh.begin() );

      Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
      double electric_energy = std::sqrt(std::accumulate(
        E.begin() , E.end() , 0. ,
        [&] ( double partial_sum , double ei ) {
          return partial_sum + ei*ei*fh.step.dx;
        }
      ));
      ee.push_back( electric_energy );

      for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
      double total_energy = energy(fh,E);
      {
        auto rhoh = fh.density();
        fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(rhoh.begin());
        fft::spectrum_ hE(c.Nx); hE.fft(E.begin());
        fft::spectrum_ hrhoc(c.Nx);
        hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
        for ( auto i=1 ; i<c.Nx ; ++i ) {
          hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
        }
        ublas::vector<double> rhoc (c.Nx,0.); hrhoc.ifft(rhoc.begin());

        for ( auto i=0 ; i<c.Nx ; ++i ) {
          total_energy += rhoc[i]*uc[i]*uc[i];
        }
      }
      H.push_back( total_energy );

      // increment time
      iter.current_time += iter.dt;
      times.push_back( iter.current_time );
      success_iterations.push_back( iter );
    }
    iterations.push_back( iter );

    ++iter.iter;
    iter.dt = std::pow( c.tol/iter.Lhfh , 0.25 )*iter.dt;
    if ( iter.current_time+iter.dt > c.Tf ) { iter.dt = c.Tf - iter.current_time; }
  } // while (  iter.current_time < c.Tf ) // end of time loop
  std::cout << "\r" << time(iter) << std::endl;

  save_data("dp4");

  auto dx_y = [&,count=0](auto const& y) mutable {
    std::stringstream ss; ss<< fh.step.dx*(count++) <<" "<<y;
    return ss.str();
  };
  c << monitoring::data( "E_"+c.name+".dat" , E , dx_y );

  ublas::vector<double> rho (c.Nx,0.);
  ublas::vector<double> rhoc(c.Nx,0.);
  {
    ublas::vector<double> rhoh = fh.density();
    fft::spectrum_ hrhoh(c.Nx); hrhoh.fft(rhoh.begin());
    fft::spectrum_ hE(c.Nx);    hE.fft(E.begin());

    fft::spectrum_ hrho(c.Nx), hrhoc(c.Nx);

    hrho[0] = I*kx[0]*hE[0] + 1.;
    hrho[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
    for ( auto i=1 ; i<c.Nx ; ++i ) {
      hrho[i]  = I*kx[i]*hE[i];
      hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
    }
    hrho.ifft(rho.begin());
  }

  c << monitoring::data( "rho_"+c.name+".dat"  , rho  , dx_y );
  c << monitoring::data( "uc_"+c.name+".dat"   , uc   , dx_y );
  c << monitoring::data( "rhoc_"+c.name+".dat" , rhoc , dx_y );

  J = fh.courant();
  for ( auto i=0 ; i<c.Nx ; ++i ) {
    J[i] += rhoc[i]*uc[i];
  }
  c << monitoring::data( "J_"+c.name+".dat" , J , dx_y );

  return 0;
}


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

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

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

//[assign_2d_point
//` Shows the usage of assign to set point coordinates

#include <iostream>
#include <iomanip>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

int main()
{
    using boost::geometry::assign_values;

    boost::geometry::model::d2::point_xy<double> p1;
    assign_values(p1, 1.2345, 2.3456);

    std::cout
        << std::setprecision(20)
        << boost::geometry::dsv(p1) << std::endl;

    return 0;
}

//]

//[assign_2d_point_output
/*`
Output:
[pre
(1.2344999999999999, 2.3456000000000001)
(1.2345, 2.3456)
]
*/
//]


////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2015 Kenjiro Sugimoto
// Released under the MIT license
// http://opensource.org/licenses/mit-license.php
////////////////////////////////////////////////////////////////////////////////

// This code implements the algorithm of the following paper. Please cite it in
// your paper if your research uses this code.
//   + K. Sugimoto and S. Kamata: "Compressive bilateral filtering", IEEE Trans.
//     Image Process., vol. 24, no. 11, pp. 3357-3369 (Nov. 2015).

#pragma once
#define _USE_MATH_DEFINES
#include <iostream>
#include <stdexcept>
#include <algorithm>
#include <string>
#include <vector>
#include <cmath>
#include <cassert>
#ifdef USE_OPENCV2
#include <opencv2/opencv.hpp>
#endif
#ifdef USE_BOOST
#include <boost/math/special_functions/erf.hpp> // for erfc_inv() only
#endif
#include "o1_spatial_gaussian_filter.hpp"

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

class compressive_bilateral_filter
{
private:
	int tone;

	// this parameter will provide sufficient accuracy.
	o1_spatial_gaussian_filter<2> gaussian;
	int K; // number of basis range kernels
	double T; // period length of periodic range kernel
	std::vector<double> sqrta;

public:
	compressive_bilateral_filter(double sigmaS,double sigmaR,double tol=0.10,int tone=256):tone(tone),gaussian(sigmaS)
	{
#ifdef USE_BOOST
		double xi=boost::math::erfc_inv(tol*tol);
#else
		// hard-coding for boost-less running
		double xi;
		     if(tol==0.05) xi=2.1378252338818511;
		else if(tol==0.10) xi=1.8213863677184496;
		else if(tol==0.20) xi=1.4522197815622468;
		else
			throw std::invalid_argument("Unsupported tolerance! ({0.05,0.10,0.20} only or use boost)");
#endif
		// estimating an optimal K
		double s=sigmaR/(tone-1.0); // normalized to dynamic range [0,1]
		K=static_cast<int>(std::ceil(xi*xi/(2.0*M_PI)+xi/(2.0*M_PI*s)-0.5));

		// estimating an optimal T
		derivative_estimated_gaussian_range_kernel_error df(s,K);
		double t1=s*xi+1.0;
		double t2=M_PI*(2*K+1)*s/xi;
		// It is better to slightly extend the original search domain D
		// because it might uncover the minimum of E(T) due to approximate error.
		const double MAGICNUM=0.03;
		T=(tone-1.0)*solve_by_bs(df,t1,t2+MAGICNUM);

		// precomputing the square root of spectrum
		double omega=2.0*M_PI/T;
		sqrta=std::vector<double>(K);
		for(int k=1;k<=K;++k)
			sqrta[k-1]=M_SQRT2*exp(-0.25*omega*omega*sigmaR*sigmaR*k*k);
	}

private:
	/// a scale-adjusted derivative of the estimated Gaussian range kernel error
	class derivative_estimated_gaussian_range_kernel_error
	{
	private:
		double sigma,kappa;
	public:
		derivative_estimated_gaussian_range_kernel_error(double sigma,int K)
			:sigma(sigma),kappa(M_PI*(2*K+1)){}
	public:
		double operator()(double T)
		{
			double phi=(T-1.0)/sigma;
			double psi=kappa*sigma/T;
			return kappa*exp(-phi*phi)-psi*psi*exp(-psi*psi);
		}
	};
	/// solve df(x)==0 by binary search
	template<class Functor>
	inline double solve_by_bs(Functor df,double x1,double x2,int loop=10)
	{
		for(int i=0;i<loop;++i)
		{
			double x=(x1+x2)/2.0;
			((0.0<=df(x))?x2:x1)=x;
		}
		return (x1+x2)/2.0;
	}

public:
#ifdef USE_OPENCV2
	/// O(1) cross/joint bilateral filtering
	/// "guide" has to have dynamic range [0,tone).
	void operator()(const cv::Mat_<double>& src,const cv::Mat_<double>& guide,cv::Mat_<double>& dst)
	{
		assert(src.size()==guide.size());
		assert(src.size()==dst.size());

		// lookup tables (discretized for fast computation)
		std::vector<double> tblC(tone);
		std::vector<double> tblS(tone);
		// component images
		cv::Mat_<cv::Vec4d> compsI(src.size());
		cv::Mat_<cv::Vec4d> compsO(src.size());

		// DC component
		const int winsz=gaussian.window_size();
		cv::Mat_<double> denom(src.size(),winsz*winsz);
		cv::Mat_<double> numer(src.size());
		gaussian.filter_xy(src,numer);

		// AC components
		double omega=2.0*M_PI/T;
		for(int k=1;k<=K;++k)
		{
			// preparing look-up tables
			double omegak=omega*k;
			for(int t=0;t<tone;++t)
			{
				double theta=omegak*t;
				tblC[t]=sqrta[k-1]*cos(theta);
				tblS[t]=sqrta[k-1]*sin(theta);
			}

			// generating k-th component images
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(guide(y,x)); // from guide image
				double c=tblC[t];
				double s=tblS[t];
				double p=src(y,x);
				compsI(y,x)=cv::Vec4d(c*p,s*p,c,s);
			}
			gaussian.filter_xy(compsI,compsO);

			// decompressing k-th components
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(guide(y,x)); // from guide image
				double c=tblC[t];
				double s=tblS[t];
				const cv::Vec4d& values=compsO(y,x);
				numer(y,x)+=c*values[0]+s*values[1];
				denom(y,x)+=c*values[2]+s*values[3];
			}
		}
		dst=numer/denom;
	}
	/// O(1) bilateral filtering
	/// "src" has to have dynamic range [0,tone).
	void operator()(const cv::Mat_<double>& src,cv::Mat_<double>& dst)
	{
		assert(src.size()==dst.size());

		// lookup tables (discretized for fast computation)
		std::vector<double> tblC(tone);
		std::vector<double> tblS(tone);
		// component images
		cv::Mat_<cv::Vec4d> compsI(src.size());
		cv::Mat_<cv::Vec4d> compsO(src.size());

		// DC component
		const int winsz=gaussian.window_size();
		cv::Mat_<double> denom(src.size(),winsz*winsz);
		cv::Mat_<double> numer(src.size());
		gaussian.filter_xy(src,numer);

		// AC components
		double omega=2.0*M_PI/T;
		for(int k=1;k<=K;++k)
		{
			// preparing look-up tables
			double omegak=omega*k;
			for(int t=0;t<tone;++t)
			{
				double theta=omegak*t;
				tblC[t]=sqrta[k-1]*cos(theta);
				tblS[t]=sqrta[k-1]*sin(theta);
			}

			// generating k-th component images
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(src(y,x));
				double c=tblC[t];
				double s=tblS[t];
				double p=src(y,x);
				compsI(y,x)=cv::Vec4d(c*p,s*p,c,s);
			}
			gaussian.filter_xy(compsI,compsO);

			// decompressing k-th components
			for(int y=0;y<src.rows;++y)
			for(int x=0;x<src.cols;++x)
			{
				int t=int(src(y,x));
				double c=tblC[t];
				double s=tblS[t];
				const cv::Vec4d& values=compsO(y,x);
				numer(y,x)+=c*values[0]+s*values[1];
				denom(y,x)+=c*values[2]+s*values[3];
			}
		}
		dst=numer/denom;
	}
#endif
};

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


// Copyright 2011-2012 Renato Tegon Forti
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// -----------------------------------------------------------------------------
// This example shows how implement a work quee (thread pool) to work with
// Boost.Application using Boost.Asio.
//
// The result will be printed on CTRL-C (Stop) signal
// -----------------------------------------------------------------------------

#include <boost/application.hpp>
#include <boost/timer/timer.hpp>

#include <iostream>
#include <iomanip>
#include <cmath>
#include <utility>
#include <vector>
#include <functional>
#include <mutex>

#include "work_queue.hpp"

using namespace boost;

using matrix_type = std::vector<std::vector<double>>;

// worker class that calculate gaussian blur
// http://en.wikipedia.org/wiki/Gaussian_blur
template< int kernelRadius = 3>
struct gaussian_blur
{
   using callback = std::function<void(const matrix_type&)>;

   explicit gaussian_blur(callback cb)
      : callback_(std::move(cb))
   {
   }

   void operator()()
   {
      boost::timer::cpu_timer timer;

      kernel2d_ = produce_gaussian_kernel(kernelRadius);

      boost::timer::cpu_times const elapsed_times(timer.elapsed());

      std::cout
         << "gaussian_blur takes:"
         <<  format(elapsed_times, 9)
         << ", for size: "
         << kernelRadius
         << std::endl;

      callback_(kernel2d_);
   }

protected:

   double gaussian(double x, double mu, double sigma)
   {
      return exp(-(((x-mu)/(sigma))*((x-mu)/(sigma)))/2.0);
   }

   matrix_type produce_gaussian_kernel(int internalKernelRadius)
   {
      // get kernel matrix
      matrix_type kernel2d(2*internalKernelRadius+1, std::vector<double>(2*internalKernelRadius+1));

      // determine sigma
      double sigma = internalKernelRadius/2.;

      // fill values
      double sum = 0;
      for (std::size_t row = 0; row < kernel2d.size(); row++)
      {
         for (std::size_t col = 0; col < kernel2d[row].size(); col++)
         {
            kernel2d[row][col] = gaussian(row, internalKernelRadius, sigma) * gaussian(col, internalKernelRadius, sigma);
            sum += kernel2d[row][col];
         }
      }

      // normalize kernel, or the image becomes dark
      for (auto & row : kernel2d)
         for (double & col : row)
            col /= sum;

      return kernel2d;
   }

private:

   callback callback_;
   matrix_type kernel2d_;
};

// application class
class myapp : work_queue<0>
{
public:

   explicit myapp(application::context& context)
      : task_count_(0), context_(context)
   {
   }

   void add_result(const matrix_type& kernel2d)
   {
      std::lock_guard<std::mutex> lock(mutex_);

      task_count_++;

      result_.push_back(kernel2d);

      if(task_count_== 3)
      {
         std::cout << "all tasks are completed, waiting ctrl-c to display the results..." << std::endl;
      }
   }

   int operator()()
   {
      // your application logic here!
      task_count_ = 0;

      //our tasks
      add_task(gaussian_blur<3>([this](const matrix_type& kernel2d) { add_result(kernel2d); }));
      add_task(gaussian_blur<6>([this](const matrix_type& kernel2d) { add_result(kernel2d); }));
      add_task(gaussian_blur<9>([this](const matrix_type& kernel2d) { add_result(kernel2d); }));

      context_.find<application::wait_for_termination_request>()->wait();

      return 0;
   }

   bool stop()
   {
      std::cout << "Result..." << std::endl;

      for(std::size_t i = 0; i < result_.size(); ++i)
      {
         std::cout << i << " : -----------------------" << std::endl;

         auto& kernel2d = result_[i];

         for (auto & row : kernel2d) {
            for (double col : row) {
               std::cout << std::setprecision(5) << std::fixed << col << " ";
            }
            std::cout << std::endl;
         }
      }

      return true;
   }

private:

   std::mutex mutex_;
   std::vector<std::vector<std::vector<double>>> result_;

   int task_count_;

   application::context& context_;
}; // myapp

int main(int /*argc*/, char */*argv*/[])
{
   application::context app_context;
   myapp app(app_context);

   application::handler<>::callback cb = [&app] { return app.stop(); };

   app_context.insert<application::termination_handler>(
      std::make_shared<application::termination_handler_default_behaviour>(cb));

   return application::launch<application::common>(app, app_context);
}


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_VECTOR3D_HPP
#define UTILITIES_GEOMETRY_VECTOR3D_HPP

#include "../UtilitiesAPI.hpp"
#include "../data/Vector.hpp"
#include "../core/Logger.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  class UTILITIES_API Vector3d{
  public:

    /// default constructor creates vector with 0, 0, 0
    Vector3d();

    /// constructor with x, y, z
    Vector3d(double x, double y, double z);

    /// copy constructor
    Vector3d(const Vector3d& other);

    /// get x
    double x() const;

    /// get y
    double y() const;

    /// get z
    double z() const;

    /// addition
    Vector3d operator+(const Vector3d& other) const;

    /// addition
    Vector3d& operator+=(const Vector3d& other);

    /// subtraction
    Vector3d operator-(const Vector3d& other) const;

    /// subtraction
    Vector3d& operator-=(const Vector3d& other);

    /// check equality
    bool operator==(const Vector3d& other) const;

    /// normalize to one
    bool normalize();

    /// get a vector which is the reverse of this
    Vector3d reverseVector() const;

    /// get length
    double length() const;

    /// set length
    bool setLength(double newLength);

    /// dot product with another Vector3d
    double dot(const Vector3d& other) const;

    /// cross product with another Vector3d
    Vector3d cross(const Vector3d& other) const;

    /// get the Vector directly
    Vector vector() const;

  private:

    REGISTER_LOGGER("utilities.Vector3d");

    Vector m_storage;

  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const Vector3d& vec);

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const std::vector<Vector3d>& vecVector);

  /// negation
  UTILITIES_API Vector3d operator-(const Vector3d& vec);

  /// multiplication by a scalar
  UTILITIES_API Vector3d operator*(double mult, const Vector3d& vec);

  // optional Vector3d
  typedef boost::optional<Vector3d> OptionalVector3d;

  // vector of Vector3d
  typedef std::vector<Vector3d> Vector3dVector;

} // openstudio

#endif //UTILITIES_GEOMETRY_VECTOR3D_HPP


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
using namespace boost::hana;

int main() {

{

//! [zero]
BOOST_HANA_CONSTANT_CHECK(zero<IntegralConstant<int>>() == int_<0>);
BOOST_HANA_CONSTEXPR_CHECK(zero<long>() == 0l);
//! [zero]

}{

//! [plus]
BOOST_HANA_CONSTANT_CHECK(plus(int_<3>, int_<5>) == int_<8>);
BOOST_HANA_CONSTEXPR_CHECK(plus(1, 2) == 3);
BOOST_HANA_CONSTEXPR_CHECK(plus(1.5f, 2.4) == 3.9);
//! [plus]

}

}