#ifndef TVMTL_MANIFOLD_GRASSMANN_HPP
#define TVMTL_MANIFOLD_GRASSMANN_HPP

#include <cmath>
#include <complex>
#include <iostream>
#include <functional>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

//own includes
#include "enumerators.hpp"
#include "matrix_utils.hpp"

namespace tvmtl {

// Specialization GRASSMANN
template <int N, int P>
struct Manifold< GRASSMANN, N, P> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	//typedef double scalar_type;
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, P>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix <scalar_type, N * P, P * (N - P) > tm_base_type;
	typedef tm_base_type& tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N*P, N*P>				deriv2_type;
	typedef deriv2_type&							deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, P * (N - P), P * (N - P) >		restricted_deriv2_type;

	// Helper Types
	typedef Eigen::PermutationMatrix<N * P, N * P, int> perm_type;

	inline static perm_type ConstructPermutationMatrix();

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static dist_type distPF_squared(cref_type x, cref_type y );
	inline static dist_type distGeod_squared( cref_type x, cref_type y );

	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	static const perm_type permutation_matrix;

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projections
	inline static void horizontal_space_projector(cref_type x, ref_type a);
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x) {};
	inline static void interpolation_postprocessing(ref_type x) {};

};

/*-----IMPLEMENTATION GRASSMANN----------*/

// Static constants, Outside definition to avoid linker error

template <int N, int P>
const MANIFOLD_TYPE Manifold <GRASSMANN, N, P>::MyType = GRASSMANN;

template <int N, int P>
const int Manifold <GRASSMANN, N, P>::manifold_dim = (N - P) * P;

template <int N, int P>
const int Manifold <GRASSMANN, N, P>::value_dim = N * P;

template <int N, int P>
const bool Manifold <GRASSMANN, N, P>::non_isometric_embedding = false;

// PermutationMatrix
template <int N, int P>
typename Manifold < GRASSMANN, N, P>::perm_type Manifold<GRASSMANN, N, P>::ConstructPermutationMatrix(){

    Eigen::Matrix<int, N * P, 1> indices;
    indices.setZero();

    int inc = 0;
    for(int i=1; i< indices.size(); ++i){
	if(i % 2 == 0)
	    inc = -P;
	else
	    inc = N;
	indices(i) = indices(i-1) + inc;
    }

    perm_type Perm(indices);
    return Perm.transpose();
}

template <int N, int P>
const typename Manifold < GRASSMANN, N, P>::perm_type Manifold<GRASSMANN, N, P>::permutation_matrix = ConstructPermutationMatrix();

// Squared GRASSMANN distance function
template <int N, int P>
inline typename Manifold <GRASSMANN, N, P>::dist_type Manifold <GRASSMANN, N, P>::dist_squared( cref_type x, cref_type y ){
    return distPF_squared(x,y);
}

template <int N, int P>
inline typename Manifold <GRASSMANN, N, P>::dist_type Manifold <GRASSMANN, N, P>::distGeod_squared( cref_type x, cref_type y ){

    // Geodesic distance
     Eigen::JacobiSVD<Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> > svd(x.transpose() * y, Eigen::ComputeThinU | Eigen::ComputeThinV);
     Eigen::Matrix<scalar_type, P, 1> sv= svd.singularValues();
     for(int i=0; i<P; ++i){
	if(sv(i)>=1.0)
	    sv(i)=0;
	else if(sv(i)<=-1.0)
	    sv(i)=M_PI;
	else
	    sv(i)=std::acos(sv(i));
    }
    #ifdef TVMTL_MANIFOLD_DEBUG_GRASSMANN
     std::cout << sv << std::endl;
    #endif

    return sv.squaredNorm();

}

template <int N, int P>
inline typename Manifold <GRASSMANN, N, P>::dist_type Manifold <GRASSMANN, N, P>::distPF_squared( cref_type x, cref_type y ){

    // Projection F-distance;
    return 0.5 * (x * x.transpose() - y * y.transpose()).squaredNorm();
}

// Derivative of Squared GRASSMANN distance w.r.t. first argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = 2.0 * (x * x.transpose() - y * y.transpose()) * x;
}
// Derivative of Squared GRASSMANN distance w.r.t. second argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    result = 2.0 * (y * y.transpose() - x * x.transpose()) * y;
}

// Second Derivative of Squared GRASSMANN distance w.r.t first argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2_type XtXId, IdXXtmYYt, XtXP;
    XtXId = Eigen::kroneckerProduct(x.transpose() * x, Eigen::Matrix<scalar_type, N, N>::Identity());
    IdXXtmYYt = Eigen::kroneckerProduct(Eigen::Matrix<scalar_type, P, P>::Identity(), x * x.transpose() - y * y.transpose());
    XtXP = Eigen::kroneckerProduct(x.transpose(), x) * permutation_matrix;
    result = 2.0 * (XtXId + XtXP + IdXXtmYYt);
}
// Second Derivative of Squared GRASSMANN distance w.r.t first and second argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = -2.0 * (Eigen::kroneckerProduct(x.transpose() * y, Eigen::Matrix<scalar_type, N, N>::Identity()) + Eigen::kroneckerProduct(x.transpose(), y) * permutation_matrix);
}
// Second Derivative of Squared GRASSMANN distance w.r.t second argument
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2xx_dist_squared(y, x, result);
}

// Exponential and Logarithm Map
template <int N, int P>
template <typename DerivedX, typename DerivedY>
inline void Manifold <GRASSMANN, N, P>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    Eigen::JacobiSVD<Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> > svd(y, Eigen::ComputeThinU | Eigen::ComputeThinV);
    value_type temp_result = x * svd.matrixV() * svd.singularValues().array().cos().matrix().asDiagonal() * svd.matrixV().transpose() + svd.matrixU() * svd.singularValues().array().sin().matrix().asDiagonal() * svd.matrixV().transpose();
    // Reorthonormalization
    Eigen::HouseholderQR<value_type> qr(temp_result);
    result = qr.householderQ() * value_type::Identity();
}

template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::log(cref_type x, cref_type y, ref_type result){
    Eigen::Matrix<scalar_type, P, P> YtX = y.transpose() * x;
    Eigen::Matrix<scalar_type, P, N> At = y.transpose() - YtX * x.transpose();

    value_type B = YtX.householderQr().solve(At).transpose();

    Eigen::JacobiSVD<Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> > svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);

    #ifdef TVMTL_MANIFOLD_DEBUG_GRASSMANN
        std::cout << "\n\nB\n" << B << std::endl;
	std::cout << "U\n" << svd.matrixU() << std::endl;
	std::cout << "S\n" << svd.singularValues() << std::endl;
	std::cout << "Vt\n" << svd.matrixV().transpose() << std::endl;
    #endif

    result = svd.matrixU() * svd.singularValues().unaryExpr(std::function<scalar_type(scalar_type)>((scalar_type(*)(scalar_type))&std::atan)).asDiagonal() * svd.matrixV().transpose();
}

// Tangent Plane restriction
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    //value_type Hproj = (Eigen::Matrix<scalar_type, N, N>::Identity() - x * x.transpose()) * x; // x - xx^tx = x - x = 0 TODO: Check of that makes sense
    //Eigen::JacobiSVD<value_type> svd(Hproj);

    // Compute X_orth by SVD
   /*
    Eigen::JacobiSVD<value_type> svd(x);
    Eigen::Matrix<scalar_type, N, N - P> xorth = svd.matrixU().rightCols(N-P);
    */

    //Compute X_orth by QR
    Eigen::HouseholderQR<value_type> qr(x);
    Eigen::Matrix<scalar_type, N, N> Q = qr.householderQ();
    Eigen::Matrix<scalar_type, N, N - P> xorth = Q.rightCols(N-P);

    int k = 0;
    for(int r = 0; r < N - P; r++)
	for(int c = 0; c < P; c++ ){
	    Eigen::Matrix<scalar_type, N, P> T = Eigen::Matrix<scalar_type, N, P>::Zero();
	    T.col(c) = xorth.col(r);
	    result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	    ++k;
	}

}

template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::horizontal_space_projector(cref_type x, ref_type a){
	    a = a - x * x.transpose() * a;
}

// Convex geodesic combinations
template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    value_type l;
    log(x, y, l);
    exp(x, l * t, result);
}

// Karcher mean implementations
template <int N, int P>
inline void Manifold<GRASSMANN, N, P>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N, int P>
inline void Manifold<GRASSMANN, N, P>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 1.0 / v.size() * L, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N, int P>
template <typename V, class... Args>
inline void Manifold<GRASSMANN, N, P>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 1.0 / numArgs * sum, temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N, int P>
template <typename V>
inline void Manifold<GRASSMANN, N, P>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N, int P>
template <typename V, class... Args>
inline void Manifold<GRASSMANN, N, P>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N, int P>
inline void Manifold <GRASSMANN, N, P>::projector(ref_type x){
	    Eigen::HouseholderQR<value_type> qr(x);
	    value_type thinQ = qr.householderQ() * value_type::Identity();
	    x = thinQ;
}

} // end namespace tvmtl

#endif


/**
 * @file burgersequation_main.cc
 * @brief NPDE homework BurgersEquation code
 * @author Oliver Rietmann
 * @date 15.04.2019
 * @copyright Developed at ETH Zurich
 */

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

#include "burgersequation.h"

int main() {
  /* SAM_LISTING_BEGIN_1 */
  const unsigned int N = 100;
  Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(N + 1, -1.0, 4.0);
  Eigen::VectorXd mu03 = BurgersEquation::solveBurgersGodunov(0.3, N);
  Eigen::VectorXd mu30 = BurgersEquation::solveBurgersGodunov(3.0, N);

  // Write the solutions to a file that can be used for plotting.
  //====================
  // Your code goes here

  //====================
  /* SAM_LISTING_END_1 */

  /* SAM_LISTING_BEGIN_2 */
  Eigen::Matrix<double, 3, 4> result = BurgersEquation::numexpBurgersGodunov();

  // Write the result to a file that can be used for plotting.
  //====================
  // Your code goes here
  //====================
  /* SAM_LISTING_END_2 */

  return 0;
}


#include <Geometry/ConvexPolygon.hpp>
#include <Geometry/LineSegment.hpp>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include <iostream>
#include <cmath>

TEST(ConvexPolygon, AddVerticesConstruction)
{
    using namespace pcv;

    ConvexPolygon<int> polygon{};

    polygon.addVertex(Eigen::Vector2i{2, 4});
    polygon.addVertex(Eigen::Vector2i{3, 5});
    polygon.addVertex(Eigen::Vector2i{4, 0});
    polygon.addVertex(Eigen::Vector2i{0, 0});

    ConvexPolygon<int> polygonReplica;
    polygonReplica.addVertex(Eigen::Vector2i{2, 4});
    polygonReplica.addVertex(Eigen::Vector2i{3, 5});
    polygonReplica.addVertex(Eigen::Vector2i{4, 0});
    polygonReplica.addVertex(Eigen::Vector2i{0, 0});

    ASSERT_EQ(polygon, polygonReplica);
}

TEST(Triagle, SignedArea)
{
    using namespace pcv;
    using namespace Eigen;

    const double negativeArea = getTwiceSignedArea2D(Vector2d{2.0, 4.0}, Vector2d{3.0, 5.0}, Vector2d{4.0, 0.0});
    ASSERT_EQ(negativeArea, -6.0);

    const int positiveArea = getTwiceSignedArea2D(Vector2i{4, 0}, Vector2i{3, 5}, Vector2i{2, 4});
    ASSERT_EQ(positiveArea, 6);
}

TEST(ConvexPolygon, PointContained)
{
    using namespace pcv;
    using namespace Eigen;

    ConvexPolygon<int> polygon{};

    polygon.addVertex(Vector2i{2, 4});
    polygon.addVertex(Vector2i{3, 5});
    polygon.addVertex(Vector2i{4, 0});
    polygon.addVertex(Vector2i{0, 0});

    // Check that all the vertices are contained in the polygon.
    ASSERT_TRUE(polygon.isPointContained(Vector2i{2, 4}.cast<double>()));
    ASSERT_TRUE(polygon.isPointContained(Vector2d{3, 5}));
    ASSERT_TRUE(polygon.isPointContained(Vector2d{4, 0}));
    ASSERT_TRUE(polygon.isPointContained(Vector2d{0, 0}));

    ASSERT_TRUE(polygon.isPointContained(Vector2d{2, 2}));
    ASSERT_TRUE(polygon.isPointContained(Vector2d{1, 1}));
    ASSERT_FALSE(polygon.isPointContained(Vector2d{5, 5}));
}

TEST(ConvexPolygon, RasterizeToEigen)
{
    using namespace pcv;
    using namespace Eigen;

    ConvexPolygon<int> polygon{};

    polygon.addVertex(Vector2i{2, 4});
    polygon.addVertex(Vector2i{3, 5});
    polygon.addVertex(Vector2i{4, 0});
    polygon.addVertex(Vector2i{0, 0});

    MatrixXi autoMask = makePolygonIntersectionEigenGrid<int>({polygon}, 7, 7);

    MatrixXi mask(7, 7);
    mask <<
        0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 1, 0, 0, 0,
        0, 0, 1, 1, 0, 0, 0,
        0, 0, 1, 1, 0, 0, 0,
        0, 1, 1, 1, 0, 0, 0,
        0, 1, 1, 1, 0, 0, 0,
        1, 1, 1, 1, 1, 0, 0;

    ASSERT_TRUE(mask == autoMask);
}

TEST(ConvexPolygon, RasterizeToOpenCV)
{
    using namespace pcv;
    using namespace Eigen;

    ConvexPolygon<int> polygon{};

    polygon.addVertex(Vector2i{2, 4});
    polygon.addVertex(Vector2i{3, 5});
    polygon.addVertex(Vector2i{4, 0});
    polygon.addVertex(Vector2i{0, 0});

    cv::Mat_<int> autoMask;
    makePolygonIntersectionOpencvGrid(
        std::vector<ConvexPolygon<int>>({polygon}), 7, 7, autoMask);

    cv::Mat_<int> mask(7, 7);
    mask = (cv::Mat_<int>(7, 7) <<
        0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 1, 0, 0, 0,
        0, 0, 1, 1, 0, 0, 0,
        0, 0, 1, 1, 0, 0, 0,
        0, 1, 1, 1, 0, 0, 0,
        0, 1, 1, 1, 0, 0, 0,
        1, 1, 1, 1, 1, 0, 0);

    cv::Mat diff = mask != autoMask;
    ASSERT_TRUE(cv::countNonZero(diff) == 0);
}

TEST(ConvexPolygon, Transform)
{
    using namespace pcv;
    using namespace Eigen;

    ConvexPolygon<double> polygon{};

    polygon.addVertex(Vector2d{0, 0});
    polygon.addVertex(Vector2d{1.5, 2});
    polygon.addVertex(Vector2d{3.5, 2});
    polygon.addVertex(Vector2d{3.5, 0});

    Matrix3d transform;
    transform <<
        2.0, 0.0, 1.0,
        0.0, 2.0, 3.0,
        0.0, 0.0, 2.0;

    polygon.transform(transform);
    const auto vertices = polygon.getVertices();

    ASSERT_DOUBLE_EQ(vertices[0].x(), 0.5);
    ASSERT_DOUBLE_EQ(vertices[0].y(), 1.5);

    ASSERT_DOUBLE_EQ(vertices[1].x(), 2.0);
    ASSERT_DOUBLE_EQ(vertices[1].y(), 3.5);

    ASSERT_DOUBLE_EQ(vertices[2].x(), 4.0);
    ASSERT_DOUBLE_EQ(vertices[2].y(), 3.5);

    ASSERT_DOUBLE_EQ(vertices[3].x(), 4.0);
    ASSERT_DOUBLE_EQ(vertices[3].y(), 1.5);
}

TEST(LineSegment, Intersection)
{
    using namespace pcv;
    using namespace Eigen;

    const auto intersection = getSegmentIntersectionPoint2(
        Vector2i{0, 0},
        Vector2i{2, 2},
        Vector2i{0, 2},
        Vector2i{2, 0});

    ASSERT_TRUE(intersection);
    ASSERT_DOUBLE_EQ(intersection->x(), 1.0);
    ASSERT_DOUBLE_EQ(intersection->y(), 1.0);
}

TEST(ConvexPolygon, Intersection)
{
    using namespace pcv;
    using namespace Eigen;

    ConvexPolygon<double> polygonA{};
    polygonA.addVertex(Vector2d{5, 14});
    polygonA.addVertex(Vector2d{9, 8});
    polygonA.addVertex(Vector2d{5, 3});
    polygonA.addVertex(Vector2d{1, 7});

    ConvexPolygon<double> polygonB{};
    polygonB.addVertex(Vector2d{9, 14});
    polygonB.addVertex(Vector2d{13, 9});
    polygonB.addVertex(Vector2d{9, 4});
    polygonB.addVertex(Vector2d{6, 9});

    auto res = polygonA.getIntersectionWith(polygonB);

    for (const auto &i : res.getVertices())
    {
        std::clog << i << std::endl << std::endl;
    }
}

int main(int argc, char** argv)
{
    ::testing::InitGoogleTest(&argc, argv);

    return RUN_ALL_TESTS();
}


#include <NTL/mat_GF2E.h>
#include <NTL/vec_GF2XVec.h>
#include <NTL/vec_long.h>
#include <NTL/BasicThreadPool.h>

NTL_START_IMPL

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

#define PAR_THRESH (40000.0)

static double
GF2E_SizeInWords()
{
   return GF2E::WordLength();
}

static
void mul_aux(Mat<GF2E>& X, const Mat<GF2E>& A, const Mat<GF2E>& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   GF2Context GF2_context;
   GF2_context.save();
   GF2EContext GF2E_context;
   GF2E_context.save();
   double sz = GF2E_SizeInWords();

   bool seq = (double(n)*double(l)*double(m)*sz*sz < PAR_THRESH);

   NTL_GEXEC_RANGE(seq, m, first, last)
   NTL_IMPORT(n)
   NTL_IMPORT(l)
   NTL_IMPORT(m)

   GF2_context.restore();
   GF2E_context.restore();

   long i, j, k;
   GF2X acc, tmp;

   Vec<GF2E> B_col;
   B_col.SetLength(l);

   for (j = first; j < last; j++) {
      for (k = 0; k < l; k++) B_col[k] = B[k][j];

      for (i = 0; i < n; i++) {
         clear(acc);
         for (k = 0; k < l; k++) {
            mul(tmp, rep(A[i][k]), rep(B_col[k]));
            add(acc, acc, tmp);
         }
         conv(X[i][j], acc);
      }
   }

   NTL_GEXEC_RANGE_END
}

void mul(mat_GF2E& X, const mat_GF2E& A, const mat_GF2E& B)
{
   if (&X == &A || &X == &B) {
      mat_GF2E tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void inv(GF2E& d, Mat<GF2E>& X, const Mat<GF2E>& A)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   const GF2XModulus& G = GF2E::modulus();

   GF2X t1, t2;
   GF2X pivot;
   GF2X pivot_inv;

   Vec< GF2XVec > M;
   // scratch space

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetSize(n, 2*GF2E::WordLength());
      for (long j = 0; j < n; j++) {
         M[i][j] = rep(A[i][j]);
      }
   }

   GF2X det;
   det = 1;

   Vec<long> P;
   P.SetLength(n);
   for (long k = 0; k < n; k++) P[k] = k;
   // records swap operations

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   bool seq = double(n)*double(n)*sz*sz < PAR_THRESH;

   bool pivoting = false;

   for (long k = 0; k < n; k++) {

      long pos = -1;

      for (long i = k; i < n; i++) {
         rem(pivot, M[i][k], G);
         if (pivot != 0) {
            InvMod(pivot_inv, pivot, G);
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
            P[k] = pos;
            pivoting = true;
         }

         MulMod(det, det, pivot, G);

         {
            // multiply row k by pivot_inv
            GF2X *y = &M[k][0];
            for (long j = 0; j < n; j++) {
               rem(t2, y[j], G);
               MulMod(y[j], t2, pivot_inv, G);
            }
            y[k] = pivot_inv;
         }

         NTL_GEXEC_RANGE(seq, n, first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         GF2_context.restore();

         GF2X *y = &M[k][0];
         GF2X t1, t2;

         for (long i = first; i < last; i++) {
            if (i == k) continue; // skip row k

            GF2X *x = &M[i][0];
            rem(t1, x[k], G);
            negate(t1, t1);
            x[k] = 0;
            if (t1 == 0) continue;

            // add t1 * row k to row i
            for (long j = 0; j < n; j++) {
               mul(t2, y[j], t1);
               add(x[j], x[j], t2);
            }
         }
         NTL_GEXEC_RANGE_END
      }
      else {
         clear(d);
         return;
      }
   }

   if (pivoting) {
      // pivot colums, using reverse swap sequence

      for (long i = 0; i < n; i++) {
         GF2X *x = &M[i][0];

         for (long k = n-1; k >= 0; k--) {
            long pos = P[k];
            if (pos != k) swap(x[pos], x[k]);
         }
      }
   }

   X.SetDims(n, n);
   for (long i = 0; i < n; i++)
      for (long j = 0; j < n; j++)
         conv(X[i][j], M[i][j]);

   conv(d, det);
}

static
void solve_impl(GF2E& d, Vec<GF2E>& X,
                const Mat<GF2E>& A, const Vec<GF2E>& b, bool trans)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   GF2X t1, t2;

   const GF2XModulus& G = GF2E::modulus();

   Vec< GF2XVec > M;

   M.SetLength(n);

   for (long i = 0; i < n; i++) {
      M[i].SetSize(n+1, 2*GF2E::WordLength());

      if (trans)
         for (long j = 0; j < n; j++) M[i][j] = rep(A[j][i]);
      else
         for (long j = 0; j < n; j++) M[i][j] = rep(A[i][j]);

      M[i][n] = rep(b[i]);
   }

   GF2X det;
   set(det);

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j <= n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         GF2_context.restore();

         GF2X t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            GF2X *x = M[i].elts() + (k+1);
            GF2X *y = M[k].elts() + (k+1);

            for (long j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (long i = n-1; i >= 0; i--) {
      clear(t1);
      for (long j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void solve(GF2E& d, Vec<GF2E>& x,
               const Mat<GF2E>& A, const Vec<GF2E>& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(GF2E& d, const Mat<GF2E>& A,
               Vec<GF2E>& x, const Vec<GF2E>& b)
{
   solve_impl(d, x, A, b, false);
}

long gauss(Mat<GF2E>& M_in, long w)
{
   GF2X t1, t2;
   GF2X piv;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const GF2XModulus& G = GF2E::modulus();

   Vec< GF2XVec > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetSize(m, 2*GF2E::WordLength());
      for (long j = 0; j < m; j++) {
         M[i][j] = rep(M_in[i][j]);
      }
   }

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   long l = 0;
   for (long k = 0; k < w && l < n; k++) {

      long pos = -1;
      for (long i = l; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(piv, M[l][k], G);
         negate(piv, piv);

         for (long j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], G);
         }

         bool seq =
            double(n-(l+1))*double(m-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(l+1), first, last)
         NTL_IMPORT(m)
         NTL_IMPORT(k)
         NTL_IMPORT(l)

         GF2_context.restore();

         GF2X t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + l+1;

            // M[i] = M[i] + M[l]*M[i,k]*piv

            MulMod(t1, M[i][k], piv, G);

            clear(M[i][k]);

            GF2X *x = M[i].elts() + (k+1);
            GF2X *y = M[l].elts() + (k+1);

            for (long j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         NTL_GEXEC_RANGE_END

         l++;
      }
   }

   for (long i = 0; i < n; i++)
      for (long j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(Mat<GF2E>& M)
{
   return gauss(M, M.NumCols());
}

void image(Mat<GF2E>& X, const Mat<GF2E>& A)
{
   Mat<GF2E> M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(Mat<GF2E>& X, const Mat<GF2E>& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   const GF2XModulus& G = GF2E::modulus();

   Mat<GF2E> M;

   transpose(M, A);
   long r = gauss(M);

   if (r == 0) {
      ident(X, m);
      return;
   }

   X.SetDims(m-r, m);

   if (m-r == 0 || m == 0) return;

   Vec<long> D;
   D.SetLength(m);
   for (long j = 0; j < m; j++) D[j] = -1;

   Vec<GF2E> inverses;
   inverses.SetLength(m);

   for (long i = 0, j = -1; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   GF2EContext GF2E_context;
   GF2E_context.save();
   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   bool seq =
      double(m-r)*double(r)*double(r)*sz*sz < PAR_THRESH;

   NTL_GEXEC_RANGE(seq, m-r, first, last)
   NTL_IMPORT(m)
   NTL_IMPORT(r)

   GF2_context.restore();
   GF2E_context.restore();

   GF2X t1, t2;
   GF2E T3;

   for (long k = first; k < last; k++) {
      Vec<GF2E>& v = X[k];
      long pos = 0;
      for (long j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            long i = D[j];

            clear(t1);

            for (long s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }

   NTL_GEXEC_RANGE_END
}

void determinant(GF2E& d, const Mat<GF2E>& M_in)
{
   GF2X t1, t2;

   const GF2XModulus& G = GF2E::modulus();

   long n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   Vec< GF2XVec > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetSize(n, 2*GF2E::WordLength());
      for (long j = 0; j < n; j++) {
         M[i][j] = rep(M_in[i][j]);
      }
   }

   GF2X det;
   set(det);

   GF2Context GF2_context;
   GF2_context.save();
   double sz = GF2E_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j < n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         GF2_context.restore();

         GF2X t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            GF2X *x = M[i].elts() + (k+1);
            GF2X *y = M[k].elts() + (k+1);

            for (long j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

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

void add(mat_GF2E& X, const mat_GF2E& A, const mat_GF2E& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

static
void mul_aux(vec_GF2E& x, const mat_GF2E& A, const vec_GF2E& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   GF2X acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_GF2E& x, const mat_GF2E& A, const vec_GF2E& b)
{
   if (&b == &x || A.alias(x)) {
      vec_GF2E tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_GF2E& x, const vec_GF2E& a, const mat_GF2E& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   GF2X acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_GF2E& x, const vec_GF2E& a, const mat_GF2E& B)
{
   if (&a == &x) {
      vec_GF2E tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);
}

void ident(mat_GF2E& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

long IsIdent(const mat_GF2E& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_GF2E& X, const mat_GF2E& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_GF2E tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void mul(mat_GF2E& X, const mat_GF2E& A, const GF2E& b_in)
{
   GF2E b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_GF2E& X, const mat_GF2E& A, GF2 b)
{
   X = A;
   if (b == 0)
      clear(X);
}

void diag(mat_GF2E& X, long n, const GF2E& d_in)
{
   GF2E d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_GF2E& A, long n, const GF2E& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_GF2E& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_GF2E& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_GF2E operator+(const mat_GF2E& a, const mat_GF2E& b)
{
   mat_GF2E res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_GF2E, res);
}

mat_GF2E operator*(const mat_GF2E& a, const mat_GF2E& b)
{
   mat_GF2E res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_GF2E, res);
}

mat_GF2E operator-(const mat_GF2E& a, const mat_GF2E& b)
{
   mat_GF2E res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_GF2E, res);
}

mat_GF2E operator-(const mat_GF2E& a)
{
   mat_GF2E res;
   negate(res, a);
   NTL_OPT_RETURN(mat_GF2E, res);
}

vec_GF2E operator*(const mat_GF2E& a, const vec_GF2E& b)
{
   vec_GF2E res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_GF2E, res);
}

vec_GF2E operator*(const vec_GF2E& a, const mat_GF2E& b)
{
   vec_GF2E res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_GF2E, res);
}

void inv(mat_GF2E& X, const mat_GF2E& A)
{
   GF2E d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_GF2E& X, const mat_GF2E& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_GF2E T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_GF2E& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra vector gradient
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_raster.h"
#include "fern/algorithm/algebra/vector/gradient.h"
#include "fern/algorithm/core/if.h"
#include "fern/algorithm/core/test/test_utils.h"

namespace fa = fern::algorithm;

BOOST_AUTO_TEST_CASE(algorithm)
{
    // Create input raster:
    // +----+----+----+
    // |  X |  1 |  2 |
    // +----+----+----+
    // |  3 |  X |  5 |
    // +----+----+----+
    // |  6 |  7 |  X |
    // +----+----+----+
    // |  9 | 10 | 11 |
    // +----+----+----+
    using MaskedRaster = fern::MaskedRaster<double, 2>;

    size_t const nr_rows = 4;
    size_t const nr_cols = 3;
    auto extents = fern::extents[nr_rows][nr_cols];

    double const cell_width = 2.0;
    double const cell_height = 3.0;
    double const west = 0.0;
    double const north = 0.0;
    MaskedRaster::Transformation transformation{{west, cell_width, north,
        cell_height}};

    MaskedRaster raster(extents, transformation);

    std::iota(raster.data(), raster.data() + raster.num_elements(), 0);
    raster.mask()[0][0] = true;
    raster.mask()[1][1] = true;
    raster.mask()[2][2] = true;

    fa::ExecutionPolicy execution_policy = fa::SequentialExecutionPolicy{};

    // Calculate gradient_x.
    {
        MaskedRaster result_we_got(extents, transformation, -9.0);
        MaskedRaster result_we_want(extents, transformation);
        result_we_want[0][0] =  -9.0;
        result_we_want[0][1] = ( 2.0 - 1.0 ) / cell_width;
        result_we_want[0][2] = ( 2.0 - 1.0 ) / cell_width;
        result_we_want[1][0] =   0.0;
        result_we_want[1][1] =  -9.0;
        result_we_want[1][2] =   0.0;
        result_we_want[2][0] = ( 7.0 - 6.0 ) / cell_width;
        result_we_want[2][1] = ( 7.0 - 6.0 ) / cell_width;
        result_we_want[2][2] =  -9.0;
        result_we_want[3][0] = (10.0 - 9.0 ) / cell_width;
        result_we_want[3][1] = (11.0 - 9.0 ) / (2 * cell_width);
        result_we_want[3][2] = (11.0 - 10.0) / cell_width;
        result_we_want.mask()[0][0] = true;
        result_we_want.mask()[1][1] = true;
        result_we_want.mask()[2][2] = true;

        fa::core::if_(execution_policy, raster.mask(), true,
            result_we_got.mask());

        fa::InputNoDataPolicies<fa::DetectNoDataByValue<fern::Mask<2>>>
             input_no_data_policy{{raster.mask(), true}};
        fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
            result_we_got.mask(), true);

        fa::algebra::gradient_x(input_no_data_policy, output_no_data_policy,
            execution_policy, raster, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Calculate gradient_y.
    {
        MaskedRaster result_we_got(extents, transformation, -9.0);
        MaskedRaster result_we_want(extents, transformation);
        result_we_want[0][0] =  -9.0;
        result_we_want[0][1] =   0.0;
        result_we_want[0][2] = ( 5.0 - 2.0) / cell_height;
        result_we_want[1][0] = ( 6.0 - 3.0) / cell_height;
        result_we_want[1][1] =  -9.0;
        result_we_want[1][2] = ( 5.0 - 2.0) / cell_height;
        result_we_want[2][0] = ( 9.0 - 3.0) / (2 * cell_height);
        result_we_want[2][1] = (10.0 - 7.0) / cell_height;
        result_we_want[2][2] =  -9.0;
        result_we_want[3][0] = ( 9.0 - 6.0) / cell_height;
        result_we_want[3][1] = (10.0 - 7.0) / cell_height;
        result_we_want[3][2] =   0.0;
        result_we_want.mask()[0][0] = true;
        result_we_want.mask()[1][1] = true;
        result_we_want.mask()[2][2] = true;

        fa::core::if_(execution_policy, raster.mask(), true,
            result_we_got.mask());

        fa::InputNoDataPolicies<fa::DetectNoDataByValue<fern::Mask<2>>>
            input_no_data_policy{{raster.mask(), true}};
        fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
            result_we_got.mask(), true);

        fa::algebra::gradient_y(input_no_data_policy, output_no_data_policy,
            execution_policy, raster, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }
}


/*
 * Copyright (C) 2015 Hamza Merzić
 *
 * 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.
 *
*/
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE DhTest

#include "dh_parameter.h"

#include <cmath>
#include <vector>

#include <boost/test/unit_test.hpp>

void CheckCloseTables(const std::vector<std::vector<float>>& vec_table_rot,
                      const std::vector<float>& vec_table_trans,
                      const DhParameter& dh_table, double eps = 0.1) {
  const double small = 0.0001;
  for(int i(0); i < 3; ++i)
    for(int j(0); j < 3; ++j) {
      double expected (vec_table_rot[i][j]),
             returned (dh_table.rotation()(i, j));
      if(std::fabs(expected) < small)
        BOOST_CHECK_SMALL(returned, small);
      else
        BOOST_CHECK_CLOSE(expected, returned, eps);
    }

  for(int i(0); i < 3; ++i) {
      double expected(vec_table_trans[i]),
             returned(static_cast<double>(dh_table.translation()[i]));
      if(std::fabs(expected) < small)
        BOOST_CHECK_SMALL(returned, small);
      else
        BOOST_CHECK_CLOSE(expected, returned, eps);
  }
}

void CheckCloseTransform(const EMatrix& R, const EVector& T,
                         const EMatrix& R_exp, const EVector& T_exp,
                         double eps = 0.1) {
  const double small = 0.0001;
  for(int i(0); i < 3; ++i)
    for(int j(0); j < 3; ++j) {
      double expected (R_exp(i, j)), returned (R(i, j));
      if(std::fabs(expected) < small)
        BOOST_CHECK_SMALL(returned, small);
      else
        BOOST_CHECK_CLOSE(expected, returned, eps);
    }

  for(int i(0); i < 3; ++i) {
      double expected (T_exp(i)), returned (T(i));
      if(std::fabs(expected) < small)
        BOOST_CHECK_SMALL(returned, small);
      else
        BOOST_CHECK_CLOSE(expected, returned, eps);
  }
}

BOOST_AUTO_TEST_CASE(constructor1) {
  const double pi_6 = M_PI / 6;
  CheckCloseTables({{0.866, 0.0,    0.5},
                    {  0.5, 0.0, -0.866},
                    {  0.0, 1.0,    0.0}},
                   {0.217, 0.125, 0},
                   DhParameter(pi_6, 0, 0.25, M_PI_2), 1.0);
}

BOOST_AUTO_TEST_CASE(constructor2) {
  const double pi_6 = M_PI / 6, pi_5 = M_PI / 5;
  CheckCloseTables({{0.866, -0.404,  0.293},
                    {  0.5,    0.7, -0.509},
                    {  0.0,  0.587,  0.809}},
                   {0.866, 0.5, 2.0},
                   DhParameter(pi_6, 2, 1, pi_5), 1.0);
}

BOOST_AUTO_TEST_CASE(transform) {
  const double pi_6 = M_PI / 6, pi_5 = M_PI / 5;
  DhParameter dh (pi_6, 2, 1, pi_5);
  EMatrix R (EMatrix::Identity());
  EVector T (1, 2, 3);
  dh.Transform(R, T);

  EMatrix R_exp;
  R_exp << 0.86603, -0.40451,   0.29389,
           0.50000,  0.70063,  -0.50904,
           0.00000,  0.58779,   0.80902;
  EVector T_exp (1.8660, 2.5, 5.0);
  CheckCloseTransform(R, T, R_exp, T_exp);
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  cout << Matrix2d::Ones() << endl;
cout << 6 * RowVector4i::Ones() << endl;

  return 0;
}


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <random>
#include <boost/numeric/ublas/tensor.hpp>

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

BOOST_AUTO_TEST_SUITE ( test_tensor_static )

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_tensor_ctor, value,  test_types)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

//    auto a1 = ublas::tensor_static<value_type, ublas::extents<>,layout_type>{};
//    BOOST_CHECK_EQUAL( a1.size() , 0ul );
//    BOOST_CHECK( a1.empty() );

    auto a2 = ublas::tensor_static<value_type, ublas::extents<1,1>,layout_type>{};
    BOOST_CHECK_EQUAL(  a2.size() , 1 );
    BOOST_CHECK( !a2.empty() );

    auto a3 = ublas::tensor_static<value_type, ublas::extents<2,1>,layout_type>{};
    BOOST_CHECK_EQUAL(  a3.size() , 2 );
    BOOST_CHECK( !a3.empty() );

    auto a4 = ublas::tensor_static<value_type, ublas::extents<1,2>,layout_type>{};
    BOOST_CHECK_EQUAL(  a4.size() , 2 );
    BOOST_CHECK( !a4.empty() );

    auto a5 = ublas::tensor_static<value_type, ublas::extents<2,1>,layout_type>{};
    BOOST_CHECK_EQUAL(  a5.size() , 2 );
    BOOST_CHECK( !a5.empty() );

    auto a6 = ublas::tensor_static<value_type, ublas::extents<4,3,2>,layout_type>{};
    BOOST_CHECK_EQUAL(  a6.size() , 4*3*2 );
    BOOST_CHECK( !a6.empty() );

    auto a7 = ublas::tensor_static<value_type, ublas::extents<4,1,2>,layout_type>{};
    BOOST_CHECK_EQUAL(  a7.size() , 4*1*2 );
    BOOST_CHECK( !a7.empty() );

}

struct fixture
{
    template<size_t... N>
    using extents_type = boost::numeric::ublas::extents<N...>;

    fixture()=default;

    std::tuple<
        extents_type<1,1>,   // 1
        extents_type<2,3>,   // 2
        extents_type<4,1,3>,  // 3
        extents_type<4,2,3>,  // 4
        extents_type<4,2,3,5>   // 5
    > extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ctor_extents, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        auto t = ublas::tensor_static<value_type, extents_type, layout_type>{};

        BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
        BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );
        if(ublas::empty(e)) {
            BOOST_CHECK       ( t.empty()    );
        }
        else{
            BOOST_CHECK       ( !t.empty()    );
        }
    });

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_copy_ctor, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        auto r = ublas::tensor_static<value_type, extents_type, layout_type>{0};

        auto t = r;
        BOOST_CHECK_EQUAL (  t.size() , r.size() );
        BOOST_CHECK_EQUAL (  t.rank() , r.rank() );
        BOOST_CHECK ( t.strides() == r.strides() );
        BOOST_CHECK ( t.extents() == r.extents() );

        if(ublas::empty(e)) {
            BOOST_CHECK       ( t.empty()    );
        }
        else{
            BOOST_CHECK       ( !t.empty()    );
        }

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_TEST( t[i] == r[i]);

    });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_copy_ctor_layout, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;
    using other_layout_type = std::conditional_t<std::is_same<ublas::layout::first_order,layout_type>::value, ublas::layout::last_order, ublas::layout::first_order>;

    for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;
        auto r = tensor_type{0};
        ublas::tensor_static<value_type, extents_type, other_layout_type> t = r;
        tensor_type q = t;

        BOOST_CHECK_EQUAL (  t.size() , r.size() );
        BOOST_CHECK_EQUAL (  t.rank() , r.rank() );
        BOOST_CHECK ( t.extents() == r.extents() );

        BOOST_CHECK_EQUAL (  q.size() , r.size() );
        BOOST_CHECK_EQUAL (  q.rank() , r.rank() );
        BOOST_CHECK ( q.strides() == r.strides() );
        BOOST_CHECK ( q.extents() == r.extents() );

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_TEST( q[i] == r[i]);

    });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_copy_move_ctor, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    auto check = [](auto const& /*unused*/, auto& e)
    {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;
        auto r = tensor_type{};
        auto t = std::move(r);
        BOOST_CHECK_EQUAL (  t.size() , ublas::product(e) );
        BOOST_CHECK_EQUAL (  t.rank() , ublas::size   (e) );

        if(ublas::empty(e)) {
            BOOST_CHECK       ( t.empty()    );
        }
        else{
            BOOST_CHECK       ( !t.empty()    );
        }

    };

    for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ctor_extents_init, value,  test_types, fixture )
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    std::random_device device{};
    std::minstd_rand0 generator(device());

    using distribution_type = std::conditional_t<std::is_integral_v<value_type>, std::uniform_int_distribution<>, std::uniform_real_distribution<> >;
    auto distribution = distribution_type(1,6);

    for_each_in_tuple(extents, [&](auto const& /*unused*/, auto const& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;

        auto r = value_type( static_cast< inner_type_t<value_type> >(distribution(generator)) );
        auto t = tensor_type{r};
        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL( t[i], r );

    });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ctor_extents_array, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;
        using container_type  = typename tensor_type::container_type;

        auto a = container_type();
        auto v = value_type {};

        for(auto& aa : a){
            aa = v;
            v += value_type{1};
        }
        auto t = tensor_type(a);
        v = value_type{};

        for(auto i = 0ul; i < t.size(); ++i, v+=value_type{1})
            BOOST_CHECK_EQUAL( t[i], v);

    });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_read_write_single_index_access, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;

        auto t = tensor_type{};
        auto v = value_type {};
        for(auto i = 0ul; i < t.size(); ++i, v+=value_type{1}){
            t[i] = v;
            BOOST_CHECK_EQUAL( t[i], v );

            t(i) = v;
            BOOST_CHECK_EQUAL( t(i), v );
        }

    });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_read_write_multi_index_access_at, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    auto check1 = [](const auto& t)
    {
        auto v = value_type{};
        for(auto k = 0ul; k < t.size(); ++k){
            BOOST_CHECK_EQUAL(t[k], v);
            v+=value_type{1};
        }
    };

    auto check2 = [](const auto& t)
    {
      std::array<unsigned,2> k = {0,0};
        auto r = std::is_same<layout_type,ublas::layout::first_order>::value ? 1 : 0;
        auto q = std::is_same<layout_type,ublas::layout::last_order >::value ? 1 : 0;
        auto v = value_type{};
        for(k[r] = 0ul; k[r] < t.size(r); ++k[r]){
            for(k[q] = 0ul; k[q] < t.size(q); ++k[q]){
                BOOST_CHECK_EQUAL(t.at(k[0],k[1]), v);
                v+=value_type{1};
            }
        }
    };

    auto check3 = [](const auto& t)
    {
        std::array<unsigned,3> k= {0,0,0};
        using op_type = std::conditional_t<std::is_same_v<layout_type,ublas::layout::first_order>, std::minus<>, std::plus<>>;
        auto r = std::is_same_v<layout_type,ublas::layout::first_order> ? 2 : 0;
        auto o = op_type{};
        auto v = value_type{};
        for(k[r] = 0ul; k[r] < t.size(r); ++k[r]){
            for(k[o(r,1)] = 0ul; k[o(r,1)] < t.size(o(r,1)); ++k[o(r,1)]){
                for(k[o(r,2)] = 0ul; k[o(r,2)] < t.size(o(r,2)); ++k[o(r,2)]){
                    BOOST_CHECK_EQUAL(t.at(k[0],k[1],k[2]), v);
                    v+=value_type{1};
                }
            }
        }
    };

    auto check4 = [](const auto& t)
    {
        std::array<unsigned,4> k= {0,0,0,0};
        using op_type = std::conditional_t<std::is_same_v<layout_type,ublas::layout::first_order>, std::minus<>, std::plus<>>;
        auto r = std::is_same_v<layout_type,ublas::layout::first_order> ? 3 : 0;
        auto o = op_type{};
        auto v = value_type{};
        for(k[r] = 0ul; k[r] < t.size(r); ++k[r]){
            for(k[o(r,1)] = 0ul; k[o(r,1)] < t.size(o(r,1)); ++k[o(r,1)]){
                for(k[o(r,2)] = 0ul; k[o(r,2)] < t.size(o(r,2)); ++k[o(r,2)]){
                    for(k[o(r,3)] = 0ul; k[o(r,3)] < t.size(o(r,3)); ++k[o(r,3)]){
                        BOOST_CHECK_EQUAL(t.at(k[0],k[1],k[2],k[3]), v);
                        v+=value_type{1};
                    }
                }
            }
        }
    };

    auto check = [check1,check2,check3,check4](auto const& /*unused*/, auto const& e) {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;
        auto t = tensor_type{};
        auto v = value_type {};
        for(auto i = 0ul; i < t.size(); ++i){
            t[i] = v;
            v+=value_type{1};
        }

        if constexpr      ( std::tuple_size_v<extents_type> == 1) check1(t);
        else if constexpr ( std::tuple_size_v<extents_type> == 2) check2(t);
        else if constexpr ( std::tuple_size_v<extents_type> == 3) check3(t);
        else if constexpr ( std::tuple_size_v<extents_type> == 4) check4(t);

    };

    for_each_in_tuple(extents,check);
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_standard_iterator, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    for_each_in_tuple(extents,[](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type, extents_type, layout_type>;

        auto v = value_type {} + value_type{1};
        auto t = tensor_type{v};

        BOOST_CHECK_EQUAL( std::distance(t.begin(),  t.end ()), t.size()  );
        BOOST_CHECK_EQUAL( std::distance(t.rbegin(), t.rend()), t.size()  );

        BOOST_CHECK_EQUAL( std::distance(t.cbegin(),  t.cend ()), t.size() );
        BOOST_CHECK_EQUAL( std::distance(t.crbegin(), t.crend()), t.size() );

        if(!t.empty()) {
            BOOST_CHECK(  t.data() ==  std::addressof( *t.begin () )  ) ;
            BOOST_CHECK(  t.data() ==  std::addressof( *t.cbegin() )  ) ;
        }
    });

}

//BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_throw, value, test_types, fixture)
//{
//  namespace ublas = boost::numeric::ublas;
//  using value_type  = typename value::first_type;
//  using layout_type = typename value::second_type;
//  using tensor_type = ublas::tensor_static<value_type, ublas::extents<5,5>, layout_type>;

//  auto t = tensor_type{};
//  auto i = ublas::index::index_type<4>{};
//  BOOST_CHECK_THROW((void)t.operator()(i,i,i), std::runtime_error);

//}

BOOST_AUTO_TEST_SUITE_END()


#include "camera_array/SystemDistributions.h"
#include <boost/foreach.hpp>

#define POSE_DIM (PoseSE3::TangentDimension)

namespace argus
{

RobotTargetDistribution::Properties::Properties( bool targetPose, bool targetVel,
                                                 bool robotPose, bool robotVel )
: sampleTargetPoses( targetPose ), sampleTargetVelocities( targetVel ),
  sampleRobotPose( robotPose ), sampleRobotVelocity( robotVel ) {}

RobotTargetDistribution::RobotTargetDistribution( Properties props )
: properties( props ), gaussian( 6 )
{}

void RobotTargetDistribution::SetMean( const RobotTargetState& b )
{
	base = b;
}

RobotTargetState RobotTargetDistribution::Sample()
{
	RobotTargetState sample( base );

	if( properties.sampleRobotPose )
	{
		PoseSE3::CovarianceMatrix poseCov =
		    base.robot.covariance.topLeftCorner( POSE_DIM, POSE_DIM );
		sample.robot.pose = SamplePose( base.robot.pose, poseCov );
	}
	if( properties.sampleRobotVelocity )
	{
		PoseSE3::CovarianceMatrix velCov =
		    base.robot.covariance.block( POSE_DIM, POSE_DIM, POSE_DIM, POSE_DIM );
		sample.robot.velocity = SampleVelocity( base.robot.velocity, velCov );
	}
	if( properties.sampleTargetPoses )
	{
		BOOST_FOREACH( const RobotTargetState::TargetMap::value_type& item, base.targets )
		{
			const std::string& name = item.first;
			const TargetState& state = item.second;
			PoseSE3::CovarianceMatrix poseCov =
			    state.covariance.topLeftCorner( POSE_DIM, POSE_DIM );
			sample.targets[ name ].pose = SamplePose( state.pose, poseCov );
		}
	}
	if( properties.sampleTargetVelocities )
	{
		BOOST_FOREACH( const RobotTargetState::TargetMap::value_type& item, base.targets )
		{
			const std::string& name = item.first;
			const TargetState& state = item.second;
			PoseSE3::CovarianceMatrix velCov =
			    state.covariance.block( POSE_DIM, POSE_DIM, POSE_DIM, POSE_DIM );
			sample.targets[ name ].velocity = SampleVelocity( state.velocity, velCov );
		}
	}
	return sample;
}

PoseSE3
RobotTargetDistribution::SamplePose( const PoseSE3& mean,
                                     const PoseSE3::CovarianceMatrix& cov )
{
	gaussian.SetCovariance( cov );
	PoseSE3::TangentVector d = gaussian.Sample(2.0); // Truncates to 3 standard deviations
	return mean * PoseSE3::Exp( d );
}

PoseSE3::TangentVector
RobotTargetDistribution::SampleVelocity( const PoseSE3::TangentVector& mean,
                                         const PoseSE3::CovarianceMatrix& cov )
{
	gaussian.SetCovariance( cov );
	return mean + gaussian.Sample();
}

}


#pragma once

#include <vector>
#include <boost/math/differentiation/autodiff.hpp>

namespace hmc {

template<class MODEL, class T>
void force_ad_1d(const MODEL& model, const std::vector<T>& x, std::vector<T> *f) {
  auto x0_fvar = boost::math::differentiation::make_fvar<double, 1>(x[0]);
  auto p = model.potential(x0_fvar);
  (*f)[0] = -p.derivative(1);
}

template<class MODEL, class T>
void force_ad_2d(const MODEL& model, const std::vector<T>& x, std::vector<T> *f) {
  auto const fvar = boost::math::differentiation::make_ftuple<double, 1, 1>(x[0], x[1]);
  auto const& x0_fvar = std::get<0>(fvar);
  auto const& x1_fvar = std::get<1>(fvar);
  auto p = model.potential(x0_fvar, x1_fvar);
  (*f)[0] = -p.derivative(1, 0);
  (*f)[1] = -p.derivative(0, 1);
}

}


#ifndef STAN_MATH_PRIM_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP
#define STAN_MATH_PRIM_SCAL_FUN_BINOMIAL_COEFFICIENT_LOG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/inv.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {
/**
 * Return the log of the binomial coefficient for the specified
 * arguments.
 *
 * The binomial coefficient, \f${N \choose n}\f$, read "N choose n", is
 * defined for \f$0 \leq n \leq N\f$ by
 *
 * \f${N \choose n} = \frac{N!}{n! (N-n)!}\f$.
 *
 * This function uses Gamma functions to define the log
 * and generalize the arguments to continuous N and n.
 *
 * \f$ \log {N \choose n}
 * = \log \ \Gamma(N+1) - \log \Gamma(n+1) - \log \Gamma(N-n+1)\f$.
 *
   \f[
   \mbox{binomial\_coefficient\_log}(x, y) =
   \begin{cases}
     \textrm{error} & \mbox{if } y > x \textrm{ or } y < 0\\
     \ln\Gamma(x+1) & \mbox{if } 0\leq y \leq x \\
     \quad -\ln\Gamma(y+1)& \\
     \quad -\ln\Gamma(x-y+1)& \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{binomial\_coefficient\_log}(x, y)}{\partial x} =
   \begin{cases}
     \textrm{error} & \mbox{if } y > x \textrm{ or } y < 0\\
     \Psi(x+1) & \mbox{if } 0\leq y \leq x \\
     \quad -\Psi(x-y+1)& \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{binomial\_coefficient\_log}(x, y)}{\partial y} =
   \begin{cases}
     \textrm{error} & \mbox{if } y > x \textrm{ or } y < 0\\
     -\Psi(y+1) & \mbox{if } 0\leq y \leq x \\
     \quad +\Psi(x-y+1)& \\[6pt]
     \textrm{NaN} & \mbox{if } x = \textrm{NaN or } y = \textrm{NaN}
   \end{cases}
   \f]
 *
 * @param N total number of objects.
 * @param n number of objects chosen.
 * @return log (N choose n).
 */
template <typename T_N, typename T_n>
inline return_type_t<T_N, T_n> binomial_coefficient_log(const T_N N,
                                                        const T_n n) {
  using std::log;
  const double CUTOFF = 1000;
  if (N - n < CUTOFF) {
    const T_N N_plus_1 = N + 1;
    return lgamma(N_plus_1) - lgamma(n + 1) - lgamma(N_plus_1 - n);
  } else {
    return_type_t<T_N, T_n> N_minus_n = N - n;
    const double one_twelfth = inv(12);
    return multiply_log(n, N_minus_n) + multiply_log((N + 0.5), N / N_minus_n)
           + one_twelfth / N - n - one_twelfth / N_minus_n - lgamma(n + 1);
  }
}

}  // namespace math
}  // namespace stan
#endif