#ifndef TVMTL_MANIFOLD_EUC_HPP
#define TVMTL_MANIFOLD_EUC_HPP

#include <cmath>
#include <Eigen/Core>

#include "enumerators.hpp"

namespace tvmtl {

// Specialization EUCLIDIAN
template < int N >
struct Manifold< EUCLIDIAN, N > {

    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 dist_type;
	typedef std::vector<double>						weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, 1>   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, N> 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, N>     deriv2_type;
	typedef deriv2_type&			     deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N, N>     restricted_deriv2_type;

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_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);

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY, typename DerivedZ>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedZ>& 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);

	// Projection to manifold
	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 EUCLIDIAN----------*/

// Static constants, Outside definition to avoid linker error

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

template <int N>
const int Manifold < EUCLIDIAN, N>::manifold_dim = N;

template <int N>
const int Manifold < EUCLIDIAN, N>::value_dim = N;

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

// Squared Euclidian distance function
template <int N>
inline typename Manifold < EUCLIDIAN, N>::dist_type Manifold < EUCLIDIAN, N>::dist_squared( cref_type x, cref_type y ){
    //value_type v = x-y;
    return (x-y).squaredNorm();
}

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

// Second Derivative of Squared Euclidian distance w.r.t first argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = 2 * deriv2_type::Identity();
}
// Second Derivative of Squared Euclidian distance w.r.t first and second argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = -2 * deriv2_type::Identity();
}
// Second Derivative of Squared Euclidian distance w.r.t second argument
template <int N>
inline void Manifold < EUCLIDIAN, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    result = 2 * deriv2_type::Identity();
}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY, typename DerivedZ>
inline void Manifold <EUCLIDIAN, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedZ>& result){
    result=x+y;
}

template <int N>
inline void Manifold <EUCLIDIAN, N>::log(cref_type x, cref_type y, ref_type result){
    result = y-x;
}

// Tangent Plane restriction
template <int N>
inline void Manifold <EUCLIDIAN, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    result = tm_base_type::Identity();
}

// Projector, cut off values outside [0,1] (if noise is added)
template <int N>
inline void Manifold <EUCLIDIAN, N>::projector(ref_type x){
    for(int i=0; i<N; ++i){
	if(x[i] > 1.0) x[i] = 1.0;
	if(x[i] < 0) x[i] = 0;
    }
}

// Convex combination along geodesic
template <int N>
inline void Manifold <EUCLIDIAN, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    result = x + t * (y-x);
}

// Karcher mean implementations
template <int N>
inline void Manifold <EUCLIDIAN, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L = value_type::Zero();
    for(int i = 0; i < v.size(); ++i)
	L += v[i];
    x = L / v.size();
}

template <int N>
inline void Manifold <EUCLIDIAN, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L = value_type::Zero();
    for(int i = 0; i < v.size(); ++i)
	L += w[i]*v[i];
    x = L / v.size();
}

template <int N>
template <typename V, class... Args>
inline void Manifold<EUCLIDIAN, N>::karcher_mean(V& x, const Args&... args){
    int numArgs = sizeof...(args);
    variadic_karcher_mean_gradient(x, args...);
    x /= numArgs;
}

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

template <int N>
template <typename V, class... Args>
inline void Manifold<EUCLIDIAN, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp = x;
    variadic_karcher_mean_gradient(temp, args...);
    x = y1 + temp;
}

} // end namespace tvmtl

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNSTAT_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNSTAT_HPP_INCLUDED
#include <nt2/statistics/functions/lognstat.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/expm1.hpp>
#include <nt2/include/functions/simd/sqr.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT   ( lognstat_, tag::cpu_
                             , (A0)(A1)(A2)(A3)
                             , (scalar_ < floating_<A0> > )
                               (scalar_ < floating_<A1> > )
                               (scalar_ < floating_<A2> > )
                               (scalar_ < floating_<A3> > )
                             )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()( A0 const& mu, A1 const& sigma
                                            , A2 & m, A3 & v) const
    {
      A0 s2 = sqr(sigma);
      m = exp(mu +Half<A0>()*s2);
      v = exp(Two<A0>()*mu+s2)*expm1(s2);
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( lognstat_, tag::cpu_
                             , (A0)(A1)(A2)
                             , (scalar_ < floating_<A0> > )
                               (scalar_ < floating_<A1> > )
                               (scalar_ < floating_<A2> > )
                             )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()( A0 const& mu, A1 const& sigma
                                            , A2 & v) const
    {
      A0 m;
      lognstat(mu, sigma, m, v);
      return m;
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( lognstat_, tag::cpu_
                             , (A0)(A1)
                             , (scalar_ < floating_<A0> > )
                               (scalar_ < floating_<A1> > )
                             )
  {
    typedef std::pair<A0,A0> result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& mu, A1 const& sigma) const
    {
      A0 m, v;
      lognstat(mu, sigma, m, v);
      return result_type(m, v);
    }
  };
} }

#endif


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program 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 General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(EDGE_MOVE_HPP)
#define EDGE_MOVE_HPP

#include <vector>									// for std::vector
#include <boost/shared_ptr.hpp>						// for boost::shared_ptr
#include <boost/weak_ptr.hpp>						// for boost::weak_ptr
//#include <boost/enable_shared_from_this.hpp>		// for boost::enable_shared_from_this
#include "mcmc_updater.hpp"		// for base class MCMCUpdater

//class ExponentialDistribution;
//typedef boost::shared_ptr<ExponentialDistribution>	ExponentialDistributionShPtr;

namespace phycas
{

class MCMCChainManager;
typedef boost::weak_ptr<MCMCChainManager>			ChainManagerWkPtr;

//typedef std::map<unsigned, std::vector<double> > PolytomyDistrMap;
//typedef std::vector<double> VecPolytomyDistr;

/*----------------------------------------------------------------------------------------------------------------------
|	An EdgeMove changes the length of just one randomly-chosen edge in the tree. An edge chosen at random is set to the
|	value Y = m*exp(`lambda'*(u - 0.5)), where m is the original length and u is a Uniform(0,1) random deviate. Under
|	this proposal scheme, the random variable Y has the following properties:
|>
|	density         f(Y) = 1/(lambda*Y)
|	cdf             F(Y) = 0.5 + (1/lambda) log(Y/m)
|	minimum         m*exp(-lambda/2)
|	maximum         m*exp(lambda/2)
|	mean            (m/lambda)[exp(lambda/2) - exp(-lambda/2)]
|	variance        (m/lambda)^2 [(lambda/2)(exp(lambda) - exp(-lambda)) - (exp(lambda/2) - exp(-lambda/2))^2]
|>
|	With a starting edge length of 1.0, the proposed edge lengths have increasing mean and variance with increasing
|	lambda, but values of lambda in the range 0.1 to 2.0 appear to be reasonable.
|>
|	lambda       mean        s.d.
|	-----------------------------
|	 0.1      1.00042     0.02888
|	 0.5      1.01045     0.14554
|	 1.0      1.04219     0.29840
|	 2.0      1.17520     0.65752
|	10.0     14.84064    29.68297
|>
*/
class EdgeMove : public MCMCUpdater
	{
	public:
									EdgeMove();
									virtual ~EdgeMove()
										{
										//std::cerr << "EdgeMove dying..." << std::endl;
										}

		// Accessors
		//
		double						getTuningParameter() const;
		//bool						addEdgeMoveProposed() const;

		// Modifiers
		//
		void						setTuningParameter(double x);
		//void						setEdgeLenDistMean(double mean);

		// Utilities
		//
		void						reset();
		//void						finalize();

		// These are virtual functions in the MCMCUpdater base class
		//
		virtual bool				update();
		virtual double				getLnHastingsRatio() const;
		virtual double				getLnJacobian() const;
		virtual void				proposeNewState();
		virtual void				revert();
		virtual void				accept();

	private:

		EdgeMove &					operator=(const EdgeMove &);	// never use - don't define

	private:

        double                      lambda;         /**< the tuning parameter used for this move */

		double						origEdgelen;	/**< Length of modified edge saved (in case revert is necessary) */
		TreeNode *					origNode;		/**< Node owning the modified edge (in case revert is necessary) */
		TreeNode *					likeRoot;		/**< Node to be used as the likelihood root (equals origNode if origNode is internal, otherwise equals origNode's parent) */

		std::vector<double>		    one_edgelen;						/**< Workspace declared here to avoid unnecessary allocs/deallocs */
	};

} // namespace phycas

#endif


#include <RcppArmadillo.h>
#include <Rmath.h>
#include<functional>
//#include <boost/math/special_functions/bessel.hpp>
#include <omp.h>
using namespace Rcpp;

// Enable C++11 via this plugin (Rcpp 0.10.3 or later)
// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::plugins(openmp)]]

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(BH)]]

const int threads = 6;//for omega//
const double log_pi = log(M_PI);
const double log_2 = log(2.0);

double bessi0_exp(double x)
{
  double ax,ans;
  double y;
  if ((ax=fabs(x)) < 3.75) {
    y=x/3.75;
    y*=y;
    ans=(1.0+y*(3.5156229+y*(3.0899424+y*(1.2067492
                                            +y*(0.2659732+y*(0.360768e-1+y*0.45813e-2))))))*exp(-ax);
  } else {
    y=3.75/ax;
    ans=(1/sqrt(ax))*(0.39894228+y*(0.1328592e-1
                                      +y*(0.225319e-2+y*(-0.157565e-2+y*(0.916281e-2
                                                                           +y*(-0.2057706e-1+y*(0.2635537e-1+y*(-0.1647633e-1
                                                                           +y*0.392377e-2))))))));
  }
  return ans;
}
double bessel_sum(arma::vec x){
  const int n = x.size();
  double total = 0;
  for(int i = 0; i < n; i++) {
    total += log(bessi0_exp(x[i]));
  }
  return total;
}

double dvm_log_scaled_sum(arma::vec a, arma::vec b, arma::vec omega_vec ){
  const int n = a.size();
  return -log_pi * n - bessel_sum(omega_vec) - sum(omega_vec % (2 * a / b));
}

double dvm_log_sum(double a, double b, double omega_1 ){
  return - log_2- log_pi - log(bessi0_exp(omega_1)) - omega_1 * (a / b + 1);
}

double dgamma_log (double x, double a, double b){
  return a * log(b) - lgamma(a) + (a - 1) * log(x) - b * x;
}

double likeliC_noj(arma::vec x, arma::vec omega_vec, int p){
  arma::vec x2 = square(x);
  arma::vec xsum = cumsum(x2);
  int p1 = p - 1;
  return dvm_log_scaled_sum(x2.subvec(2,p1),xsum.subvec(2,p1),omega_vec.subvec(1,p1-1)) + dvm_log_sum(x[1],pow(xsum[1],0.5),omega_vec[0]);
}

double likeli_omega_omp(arma::vec omega_vec, arma::mat beta,int nr,int p){
  double s_beta = 0;
  omp_set_num_threads(threads);
  #pragma omp parallel for reduction(+:s_beta)

  for(int i = 0; i < nr ; i++){
    arma::vec temp = vectorise(beta.row(i));
    s_beta += likeliC_noj(temp,omega_vec,p);
  }
  return s_beta;
}

// [[Rcpp::export]]
List update_omega(arma::vec dummy, arma::vec omega_vec,arma::mat beta, int nr,double a, double b,double omega_sd,int p ){
  double omega = omega_vec[0];
  double eta = log(omega);
  double eta_new = omega_sd * as_scalar(arma::randn(1)) + eta;
  double omega_new = exp(eta_new);
  double accept;
  int omega_accept = 1;
  arma::vec omega_vec_new = omega_new * dummy;

  accept = exp(likeli_omega_omp(omega_vec_new,beta,nr,p) + eta_new  + dgamma_log(omega_new,a,b) -
    likeli_omega_omp(omega_vec,beta,nr,p) - eta - dgamma_log(omega,a,b));

  if(accept < as_scalar(arma::randu(1))){
    omega_vec_new = omega_vec;
    omega_accept = 0;
  }
  return List::create(omega_vec_new,omega_accept);
}


#pragma once

#include <Eigen/Dense>

#include <functional>
#include <string>

#ifdef POLYFEM_OPENCL
#include <rbf_interpolate.hpp>
#endif

namespace polyfem
{
	class RBFInterpolation
	{
	public:
		RBFInterpolation() { }
		RBFInterpolation(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const std::function<double(double)> &rbf);
		void init(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const std::function<double(double)> &rbf);

		RBFInterpolation(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const std::string &rbf, const double eps);
		void init(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const std::string &rbf, const double eps);

		Eigen::MatrixXd interpolate(const Eigen::MatrixXd &pts) const;

	private:
#ifdef POLYFEM_OPENCL
		int verbose_ = 0;
		const std::string rbfcl_ = "GA";
		bool opt_ = false;
		bool unit_cube_  = false;
		int num_threads_ = -1;

		std::vector<rbf_pum::RBFData> data_;
#else
		Eigen::MatrixXd centers_;
		Eigen::MatrixXd weights_;

		std::function<double(double)> rbf_;
#endif
	};
}


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>
#include "../../src/metric.hpp"
#include "spatial_test_fixtures.hpp"

BOOST_AUTO_TEST_CASE(test_difference_bracket)
{
  bracket_minus<int2, int>
    diff = details::with_builtin_difference<point_multiset<2, int2> >()
    (point_multiset<2, int2>());
  int2 p(0, 1);
  int2 q(2, 0);
  BOOST_CHECK_EQUAL(diff(0, p, q), -2);
  BOOST_CHECK_EQUAL(diff(1, p, q), 1);
}

BOOST_AUTO_TEST_CASE(test_difference_paren)
{
  typedef point_multiset<2, int2, paren_less<int2> > pointset_type;
  paren_minus<int2, int>
    diff = details::with_builtin_difference<pointset_type>()
    (pointset_type());
  int2 p(0, 1);
  int2 q(2, 0);
  BOOST_CHECK_EQUAL(diff(0, p, q), -2);
  BOOST_CHECK_EQUAL(diff(1, p, q), 1);
}

BOOST_AUTO_TEST_CASE(test_difference_iterator)
{
  typedef point_multiset<2, int2, iterator_less<int2> > pointset_type;
  iterator_minus<int2, int>
    diff = details::with_builtin_difference<pointset_type>()
    (pointset_type());
  int2 p(0, 1);
  int2 q(2, 0);
  BOOST_CHECK_EQUAL(diff(0, p, q), -2);
  BOOST_CHECK_EQUAL(diff(1, p, q), 1);
}

BOOST_AUTO_TEST_CASE(test_difference_accessor)
{
  typedef point_multiset<4, quad, accessor_less<quad_access, quad> > pointset_type;
  accessor_minus<quad_access, quad, int>
    diff = details::with_builtin_difference<pointset_type>()
    (pointset_type());
  quad p(0, 1, 0, 0);
  quad q(2, 0, 0, 0);
  BOOST_CHECK_EQUAL(diff(0, p, q), -2);
  BOOST_CHECK_EQUAL(diff(1, p, q), 1);
}

BOOST_AUTO_TEST_CASE(test_euclid_distance_to_key)
{
  {
    // distance between 2 points at the same position should be null.
    double6 x; std::fill(x.begin(), x.end(), .0);
    double r = math::euclid_distance_to_key
      <double6, bracket_minus<double6, double>, double>
      (6, x, x, bracket_minus<double6, double>());
    BOOST_CHECK_CLOSE(r, .0, .000000000001);
    std::fill(x.begin(), x.end(), -1.);
    r = math::euclid_distance_to_key
      <double6, bracket_minus<double6, double>, double>
      (6, x, x, bracket_minus<double6, double>());
    BOOST_CHECK_CLOSE(r, .0, .000000000001);
    std::fill(x.begin(), x.end(), 1.);
    r = math::euclid_distance_to_key
      <double6, bracket_minus<double6, double>, double>
      (6, x, x, bracket_minus<double6, double>());
    BOOST_CHECK_CLOSE(r, .0, .000000000001);
  }
  {
    // Distance between 2 points at different positions in 3D
    for (int i=0; i<100; ++i)
      {
        double6 p = make_double6(drand(), drand(), drand(),
                                 drand(), drand(), drand());
        double6 q = make_double6(drand(), drand(), drand(),
                                 drand(), drand(), drand());
        double dist = math::euclid_distance_to_key
          <double6, bracket_minus<double6, double>, double>
          (6, p, q, bracket_minus<double6, double>());
        using namespace ::std;
        double other_dist = sqrt((p[0] - q[0]) * (p[0] - q[0])
                                 + (p[1] - q[1]) * (p[1] - q[1])
                                 + (p[2] - q[2]) * (p[2] - q[2])
                                 + (p[3] - q[3]) * (p[3] - q[3])
                                 + (p[4] - q[4]) * (p[4] - q[4])
                                 + (p[5] - q[5]) * (p[5] - q[5]));
        BOOST_CHECK_CLOSE(dist, other_dist, .000000000001);
      }
  }
}

BOOST_AUTO_TEST_CASE( test_euclidian_square_distance_to_key )
{
  {
    // distance between 2 points at the same position should be null.
    quad x(0, 0, 0, 0);
    int r = math::square_euclid_distance_to_key
      <quad, accessor_minus<quad_access, quad, int>, int>
      (4, x, x, accessor_minus<quad_access, quad, int>());
    BOOST_CHECK_EQUAL(r, 0);
    x = quad(1, 1, 1, 1);
    r = math::square_euclid_distance_to_key
      <quad, accessor_minus<quad_access, quad, int>, int>
      (4, x, x, accessor_minus<quad_access, quad, int>());
    BOOST_CHECK_EQUAL(r, 0);
    x = quad(-1, -1, -1, -1);
    r = math::square_euclid_distance_to_key
      <quad, accessor_minus<quad_access, quad, int>, int>
      (4, x, x, accessor_minus<quad_access, quad, int>());
    BOOST_CHECK_EQUAL(r, 0);
  }
  {
    // Distance between 2 points at different positions in 3D
    for (int i=0; i<100; ++i)
      {
        quad p, q;
        p.x = std::rand() % 80 - 40;
        p.y = std::rand() % 80 - 40;
        p.z = std::rand() % 80 - 40;
        p.w = std::rand() % 80 - 40;
        q.x = std::rand() % 80 - 40;
        q.y = std::rand() % 80 - 40;
        q.z = std::rand() % 80 - 40;
        q.w = std::rand() % 80 - 40;
        int dist = math::square_euclid_distance_to_key
          <quad, accessor_minus<quad_access, quad, int>, int>
          (4, p, q, accessor_minus<quad_access, quad, int>());
        int other_dist = (p.x-q.x)*(p.x-q.x) + (p.y-q.y)*(p.y-q.y)
          + (p.z-q.z)*(p.z-q.z) + (p.w-q.w)*(p.w-q.w);
        BOOST_CHECK_EQUAL(dist, other_dist);
      }
  }
}

BOOST_AUTO_TEST_CASE( test_manhattan_distance_to_key )
{
  {
    // distance between 2 points at the same position should be null.
    quad x(0, 0, 0, 0);
    int r = math::manhattan_distance_to_key
      <quad, accessor_minus<quad_access, quad, int>, int>
      (4, x, x, accessor_minus<quad_access, quad, int>());
    BOOST_CHECK_EQUAL(r, 0);
    x = quad(1, 1, 1, 1);
    r = math::manhattan_distance_to_key
      <quad, accessor_minus<quad_access, quad, int>, int>
      (4, x, x, accessor_minus<quad_access, quad, int>());
    BOOST_CHECK_EQUAL(r, 0);
    x = quad(-1, -1, -1, -1);
    r = math::manhattan_distance_to_key
      <quad, accessor_minus<quad_access, quad, int>, int>
      (4, x, x, accessor_minus<quad_access, quad, int>());
    BOOST_CHECK_EQUAL(r, 0);
  }
  {
    // Distance between 2 points at different positions in 3D
    for (int i=0; i<100; ++i)
      {
        quad p, q;
        p.x = std::rand() % 80 - 40;
        p.y = std::rand() % 80 - 40;
        p.z = std::rand() % 80 - 40;
        p.w = std::rand() % 80 - 40;
        q.x = std::rand() % 80 - 40;
        q.y = std::rand() % 80 - 40;
        q.z = std::rand() % 80 - 40;
        q.w = std::rand() % 80 - 40;
        int dist = math::manhattan_distance_to_key
          <quad, accessor_minus<quad_access, quad, int>, int>
          (4, p, q, accessor_minus<quad_access, quad, int>());
        using namespace ::std;
        int other_dist = abs(p.x-q.x) + abs(p.y-q.y)
          + abs(p.z-q.z) + abs(p.w-q.w);
        BOOST_CHECK_EQUAL(dist, other_dist);
      }
  }
}


#include <fstream>
#include <iostream>
#include <unordered_set>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/graphviz.hpp>

class ShortestPathHeuristic {
 public:
  ShortestPathHeuristic(size_t dimx, size_t dimy,
                        const std::unordered_set<Location>& obstacles)
      : m_shortestDistance(nullptr), m_dimx(dimx), m_dimy(dimy) {
    searchGraph_t searchGraph;

    // add vertices
    for (size_t x = 0; x < dimx; ++x) {
      for (size_t y = 0; y < dimy; ++y) {
        boost::add_vertex(searchGraph);
      }
    }

    // add edges
    for (size_t x = 0; x < dimx; ++x) {
      for (size_t y = 0; y < dimy; ++y) {
        Location l(x, y);
        if (obstacles.find(l) == obstacles.end()) {
          Location right(x + 1, y);
          if (x < dimx - 1 && obstacles.find(right) == obstacles.end()) {
            auto e =
                boost::add_edge(locToVert(l), locToVert(right), searchGraph);
            searchGraph[e.first].weight = 1;
          }
          Location below(x, y + 1);
          if (y < dimy - 1 && obstacles.find(below) == obstacles.end()) {
            auto e =
                boost::add_edge(locToVert(l), locToVert(below), searchGraph);
            searchGraph[e.first].weight = 1;
          }
        }
      }
    }

    writeDotFile(searchGraph, "searchGraph.dot");

    m_shortestDistance = new distanceMatrix_t(boost::num_vertices(searchGraph));
    distanceMatrixMap_t distanceMap(*m_shortestDistance, searchGraph);
    // The following generates a clang-tidy error, see
    // https://svn.boost.org/trac10/ticket/10830
    boost::floyd_warshall_all_pairs_shortest_paths(
        searchGraph, distanceMap,
        boost::weight_map(boost::get(&Edge::weight, searchGraph)));
  }

  ~ShortestPathHeuristic() { delete m_shortestDistance; }

  int getValue(const Location& a, const Location& b) {
    vertex_t idx1 = locToVert(a);
    vertex_t idx2 = locToVert(b);
    return (*m_shortestDistance)[idx1][idx2];
  }

 private:
  size_t locToVert(const Location& l) const { return l.x + m_dimx * l.y; }

  Location idxToLoc(size_t idx) {
    int x = idx % m_dimx;
    int y = idx / m_dimx;
    return Location(x, y);
  }

 private:
  typedef boost::adjacency_list_traits<boost::vecS, boost::vecS,
                                       boost::undirectedS>
      searchGraphTraits_t;
  typedef searchGraphTraits_t::vertex_descriptor vertex_t;
  typedef searchGraphTraits_t::edge_descriptor edge_t;

  struct Vertex {};

  struct Edge {
    int weight;
  };

  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                                Vertex, Edge>
      searchGraph_t;
  typedef boost::exterior_vertex_property<searchGraph_t, int>
      distanceProperty_t;
  typedef distanceProperty_t::matrix_type distanceMatrix_t;
  typedef distanceProperty_t::matrix_map_type distanceMatrixMap_t;

  class VertexDotWriter {
   public:
    explicit VertexDotWriter(const searchGraph_t& graph, size_t dimx)
        : m_graph(graph), m_dimx(dimx) {}

    void operator()(std::ostream& out, const vertex_t& v) const {
      static const float DX = 100;
      static const float DY = 100;
      out << "[label=\"";
      int x = v % m_dimx;
      int y = v / m_dimx;
      out << "\" pos=\"" << x * DX << "," << y * DY << "!\"]";
    }

   private:
    const searchGraph_t& m_graph;
    size_t m_dimx;
  };

  class EdgeDotWriter {
   public:
    explicit EdgeDotWriter(const searchGraph_t& graph) : m_graph(graph) {}

    void operator()(std::ostream& out, const edge_t& e) const {
      out << "[label=\"" << m_graph[e].weight << "\"]";
    }

   private:
    const searchGraph_t& m_graph;
  };

 private:
  void writeDotFile(const searchGraph_t& graph, const std::string& fileName) {
    VertexDotWriter vw(graph, m_dimx);
    EdgeDotWriter ew(graph);
    std::ofstream dotFile(fileName);
    boost::write_graphviz(dotFile, graph, vw, ew);
  }

 private:
  distanceMatrix_t* m_shortestDistance;
  size_t m_dimx;
  size_t m_dimy;
};


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

// check memory allocation in some method
#define EIGEN_RUNTIME_NO_MALLOC

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE RBInertiad ABinertiad test
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;

const double TOL = 0.00001;

bool isUpperNull(const Eigen::Matrix3d & m)
{
  using namespace Eigen;
  return (Matrix3d(m.triangularView<StrictlyUpper>()).array() == 0.).all();
}

BOOST_AUTO_TEST_CASE(RBInertiadTest)
{
  using namespace Eigen;
  using namespace sva;

  double mass = 1.;
  Matrix3d I;
  I << 1., 2., 3., 2., 1., 4., 3., 4., 1.;
  Vector3d h = Vector3d::Random() * 100.;

  // Parametrized constructor double Vector3d Matrix3d
  RBInertiad rb2(mass, h, I);

  BOOST_CHECK_EQUAL(rb2.mass(), mass);
  BOOST_CHECK_EQUAL(rb2.momentum(), h);
  BOOST_CHECK_EQUAL(rb2.inertia(), I);
  BOOST_CHECK(isUpperNull(rb2.lowerTriangularInertia()));

  // Parametrized constructor double Vector3d Matrix3d
  RBInertiad rb3(mass, h, I.triangularView<Lower>());

  BOOST_CHECK_EQUAL(rb3.mass(), mass);
  BOOST_CHECK_EQUAL(rb3.momentum(), h);
  BOOST_CHECK_EQUAL(rb3.inertia(), I);
  BOOST_CHECK(isUpperNull(rb3.lowerTriangularInertia()));

  // rbI + rbI
  RBInertiad rb4 = rb2 + rb3;

  BOOST_CHECK_EQUAL(rb4.mass(), mass + mass);
  BOOST_CHECK_EQUAL(rb4.momentum(), h + h);
  BOOST_CHECK_EQUAL(rb4.inertia(), I + I);
  BOOST_CHECK(isUpperNull(rb4.lowerTriangularInertia()));

  // alpha * rbI
  RBInertiad rb5 = 2. * rb2;

  BOOST_CHECK_EQUAL(rb5.mass(), 2. * mass);
  BOOST_CHECK_EQUAL(rb5.momentum(), 2. * h);
  BOOST_CHECK_EQUAL(rb5.inertia(), 2. * I);
  BOOST_CHECK(isUpperNull(rb5.lowerTriangularInertia()));

  // rbI * alpha
  RBInertiad rb6 = rb2 * 2.;

  BOOST_CHECK_EQUAL(rb6.mass(), 2. * mass);
  BOOST_CHECK_EQUAL(rb6.momentum(), 2. * h);
  BOOST_CHECK_EQUAL(rb6.inertia(), 2. * I);
  BOOST_CHECK(isUpperNull(rb6.lowerTriangularInertia()));

  // rbI - rbI
  RBInertiad rb7 = rb2 - rb3;

  BOOST_CHECK_EQUAL(rb7.mass(), mass - mass);
  BOOST_CHECK_EQUAL(rb7.momentum(), h - h);
  BOOST_CHECK_EQUAL(rb7.inertia(), I - I);
  BOOST_CHECK(isUpperNull(rb7.lowerTriangularInertia()));

  // -rbI
  RBInertiad rb8 = -rb2;

  BOOST_CHECK_EQUAL(rb8, rb2 * -1.);
  BOOST_CHECK(isUpperNull(rb8.lowerTriangularInertia()));

  // rbI += rbI
  RBInertiad rb9(rb2);
  rb9 += rb3;

  BOOST_CHECK_EQUAL(rb9, rb2 + rb3);
  BOOST_CHECK(isUpperNull(rb9.lowerTriangularInertia()));

  // rbI -= rbI
  RBInertiad rb10(rb2);
  rb10 -= rb3;

  BOOST_CHECK_EQUAL(rb10, rb2 - rb3);
  BOOST_CHECK(isUpperNull(rb10.lowerTriangularInertia()));

  // ==
  BOOST_CHECK_EQUAL(rb2, rb2);
  BOOST_CHECK_NE(rb2, rb6);

  // !=
  BOOST_CHECK(rb2 != rb6);
  BOOST_CHECK(!(rb2 != rb2));
}

BOOST_AUTO_TEST_CASE(ABInertiadTest)
{
  using namespace Eigen;
  using namespace sva;

  Matrix3d M, H, I;
  M << 1., 2., 3., 2., 1., 4., 3., 4., 1.;
  H = Matrix3d::Random() * 100.;
  I << 1., 2., 3., 2., 1., 4., 3., 4., 1.;

  // Parametrized constructor double Vector3d Matrix3d
  ABInertiad ab1(M, H, I);

  BOOST_CHECK_EQUAL(ab1.massMatrix(), M);
  BOOST_CHECK(isUpperNull(ab1.lowerTriangularMassMatrix()));
  BOOST_CHECK_EQUAL(ab1.gInertia(), H);
  BOOST_CHECK_EQUAL(ab1.inertia(), I);
  BOOST_CHECK(isUpperNull(ab1.lowerTriangularInertia()));

  // Parametrized constructor double Vector3d Matrix3d
  ABInertiad ab2(M.triangularView<Lower>(), H, I.triangularView<Lower>());

  BOOST_CHECK_EQUAL(ab2.massMatrix(), M);
  BOOST_CHECK(isUpperNull(ab2.lowerTriangularMassMatrix()));
  BOOST_CHECK_EQUAL(ab2.gInertia(), H);
  BOOST_CHECK_EQUAL(ab2.inertia(), I);
  BOOST_CHECK(isUpperNull(ab2.lowerTriangularInertia()));

  // abI + abI
  ABInertiad ab3 = ab1 + ab2;

  BOOST_CHECK_EQUAL(ab3.massMatrix(), M + M);
  BOOST_CHECK(isUpperNull(ab3.lowerTriangularMassMatrix()));
  BOOST_CHECK_EQUAL(ab3.gInertia(), H + H);
  BOOST_CHECK_EQUAL(ab3.inertia(), I + I);
  BOOST_CHECK(isUpperNull(ab3.lowerTriangularInertia()));

  // alpha * rbI
  ABInertiad ab4 = 2. * ab2;

  BOOST_CHECK_EQUAL(ab4.massMatrix(), 2. * M);
  BOOST_CHECK(isUpperNull(ab4.lowerTriangularMassMatrix()));
  BOOST_CHECK_EQUAL(ab4.gInertia(), 2. * H);
  BOOST_CHECK_EQUAL(ab4.inertia(), 2. * I);
  BOOST_CHECK(isUpperNull(ab4.lowerTriangularInertia()));

  // abI * alpha
  ABInertiad ab5 = ab2 * 2.;

  BOOST_CHECK_EQUAL(ab5.massMatrix(), 2. * M);
  BOOST_CHECK(isUpperNull(ab5.lowerTriangularMassMatrix()));
  BOOST_CHECK_EQUAL(ab5.gInertia(), 2. * H);
  BOOST_CHECK_EQUAL(ab5.inertia(), 2. * I);
  BOOST_CHECK(isUpperNull(ab5.lowerTriangularInertia()));

  // abI - abI
  ABInertiad ab6 = ab1 - ab2;

  BOOST_CHECK_EQUAL(ab6.massMatrix(), M - M);
  BOOST_CHECK(isUpperNull(ab6.lowerTriangularMassMatrix()));
  BOOST_CHECK_EQUAL(ab6.gInertia(), H - H);
  BOOST_CHECK_EQUAL(ab6.inertia(), I - I);
  BOOST_CHECK(isUpperNull(ab6.lowerTriangularInertia()));

  // -abI
  ABInertiad ab7 = -ab1;
  BOOST_CHECK_EQUAL(ab7, ab1 * -1.);
  BOOST_CHECK(isUpperNull(ab7.lowerTriangularMassMatrix()));
  BOOST_CHECK(isUpperNull(ab7.lowerTriangularInertia()));

  // abI += abI
  ABInertiad ab8(ab1);
  ab8 += ab2;

  BOOST_CHECK_EQUAL(ab8, ab1 + ab2);
  BOOST_CHECK(isUpperNull(ab8.lowerTriangularMassMatrix()));
  BOOST_CHECK(isUpperNull(ab8.lowerTriangularInertia()));

  // abI -= abI
  ABInertiad ab9(ab1);
  ab9 -= ab2;

  BOOST_CHECK_EQUAL(ab9, ab1 - ab2);
  BOOST_CHECK(isUpperNull(ab9.lowerTriangularMassMatrix()));
  BOOST_CHECK(isUpperNull(ab9.lowerTriangularInertia()));

  // ==
  BOOST_CHECK_EQUAL(ab2, ab2);
  BOOST_CHECK_NE(ab2, ab5);

  // !=
  BOOST_CHECK(ab2 != ab5);
  BOOST_CHECK(!(ab2 != ab2));
}

BOOST_AUTO_TEST_CASE(RBInertiadLeftOperatorsTest)
{
  using namespace Eigen;
  using namespace sva;
  double mass = 1.;
  Matrix3d I;
  I << 1., 2., 3., 2., 1., 4., 3., 4., 1.;
  Vector3d h = Vector3d::Random() * 100.;
  RBInertiad rb(mass, h, I);
  Matrix6d rb6d = rb.matrix();

  Vector3d w, v;
  w = Vector3d::Random() * 100.;
  v = Vector3d::Random() * 100.;
  sva::MotionVecd mVec(w, v);
  Vector6d mVec6d = mVec.vector();

  // RBInertiad * MotionVecd
  ForceVecd fVec = rb * mVec;
  Vector6d fVec6d(rb6d * mVec6d);

  BOOST_CHECK_SMALL((fVec6d - fVec.vector()).array().abs().sum(), TOL);

  // vectorized version
  Matrix6Xd mVec6Xd(6, 2);
  Matrix6Xd fVecRes6Xd(6, 2);
  mVec6Xd << mVec.vector(), mVec.vector();

  internal::set_is_malloc_allowed(false);
  rb.mul(mVec6Xd, fVecRes6Xd);
  internal::set_is_malloc_allowed(true);

#ifdef __i386__
  BOOST_CHECK_SMALL((fVec.vector() - fVecRes6Xd.col(0)).array().abs().sum(), TOL);
#else
  BOOST_CHECK_EQUAL(fVec.vector(), fVecRes6Xd.col(0));
#endif
  BOOST_CHECK_EQUAL(fVecRes6Xd.col(0), fVecRes6Xd.col(1));
}

BOOST_AUTO_TEST_CASE(ABInertiadLeftOperatorsTest)
{
  using namespace Eigen;
  using namespace sva;
  Matrix3d M, H, I;
  M << 1., 2., 3., 2., 1., 4., 3., 4., 1.;
  H = Matrix3d::Random() * 100.;
  I << 1., 2., 3., 2., 1., 4., 3., 4., 1.;

  ABInertiad ab(M, H, I);
  Matrix6d ab6d = ab.matrix();

  double mass = 1.;
  Vector3d h = Vector3d::Random() * 100.;
  RBInertiad rb(mass, h, I);
  Matrix6d rb6d = rb.matrix();

  Vector3d w, v;
  w = Vector3d::Random() * 100.;
  v = Vector3d::Random() * 100.;
  sva::MotionVecd mVec(w, v);
  Vector6d mVec6d = mVec.vector();

  // ABInertiad + RBInertiad
  ABInertiad abRes = ab + rb;
  Matrix6d abRes6d = ab6d + rb6d;

  BOOST_CHECK_SMALL((abRes6d - abRes.matrix()).array().abs().sum(), TOL);
  BOOST_CHECK(isUpperNull(abRes.lowerTriangularMassMatrix()));
  BOOST_CHECK(isUpperNull(abRes.lowerTriangularInertia()));

  // ABInertiad * MotionVecd
  ForceVecd fVec = ab * mVec;
  Vector6d fVec6d(ab6d * mVec6d);

  BOOST_CHECK_SMALL((fVec6d - fVec.vector()).array().abs().sum(), TOL);

  // vectorized version
  Matrix6Xd mVec6Xd(6, 2);
  Matrix6Xd fVecRes6Xd(6, 2);
  mVec6Xd << mVec.vector(), mVec.vector();

  internal::set_is_malloc_allowed(false);
  ab.mul(mVec6Xd, fVecRes6Xd);
  internal::set_is_malloc_allowed(true);

#ifdef __i386__
  BOOST_CHECK_SMALL((fVec.vector() - fVecRes6Xd.col(0)).array().abs().sum(), TOL);
#else
  BOOST_CHECK_EQUAL(fVec.vector(), fVecRes6Xd.col(0));
#endif
  BOOST_CHECK_EQUAL(fVecRes6Xd.col(0), fVecRes6Xd.col(1));
}


#ifndef EIGEN_SHORT_HPP
#define EIGEN_SHORT_HPP

#include <Eigen/Dense>

template <class T>
    using map = Eigen::Map<T>;

template <int M = Eigen::Dynamic, int N = Eigen::Dynamic>
    using mat = Eigen::Matrix<double, M, N>;

template <int M = Eigen::Dynamic, int N = Eigen::Dynamic>
    using rmat = Eigen::Ref<mat<M,N>>;

template <int M = Eigen::Dynamic, int N = Eigen::Dynamic>
    using cmat = const Eigen::Ref<const mat<M,N>>;

template <int M = Eigen::Dynamic>
    using vec = mat<M,1>;

template <int M = Eigen::Dynamic>
    using rvec = rmat<M,1>;

template <int M = Eigen::Dynamic>
    using cvec = cmat<M,1>;

using quat = Eigen::Quaterniond;

#endif


/**
 * \file TestFEM.cpp
 * \author Norihiro Watanabe
 * \date   2012-08-03
 *
 * \copyright
 * Copyright (c) 2013, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <gtest/gtest.h>

#include <vector>
#include <cmath>
#ifdef OGS_USE_EIGEN
#include <Eigen/Eigen>
#endif

#include "MeshLib/Elements/Quad.h"
#include "NumLib/Fem/CoordinatesMapping/ShapeMatrices.h"
#include "NumLib/Fem/FiniteElement/C0IsoparametricElements.h"

#include "Tests/TestTools.h"

using namespace NumLib;

namespace
{

static const unsigned dim = 2;
static const unsigned e_nnodes = 4;

template <class T_MATRIX_TYPES>
class NumLibFemIsoQuad4Test : public ::testing::Test
{
 public:
    // Matrix types
    typedef typename T_MATRIX_TYPES::NodalMatrixType NodalMatrix;
    typedef typename T_MATRIX_TYPES::NodalVectorType NodalVector;
    typedef typename T_MATRIX_TYPES::DimNodalMatrixType DimNodalMatrix;
    typedef typename T_MATRIX_TYPES::DimMatrixType DimMatrix;
    // Finite element type
    typedef typename NumLib::FeQUAD4<NodalVector, DimNodalMatrix, DimMatrix>::type FeQUAD4Type;
    // Shape matrix data type
    typedef typename FeQUAD4Type::ShapeMatricesType ShapeMatricesType;

 public:
    NumLibFemIsoQuad4Test() :
        D(dim, dim),
        expectedM(e_nnodes,e_nnodes),
        expectedK(e_nnodes,e_nnodes),
        integration_method(2)
    {
        // create a quad element used for testing
        unitSquareQuad   = createSquareQuad(1.0);

        // set a conductivity tensor
        setIdentityMatrix(dim, D);
        D *= conductivity;

        // set expected matrices
        setExpectedMassMatrix(expectedM);
        setExpectedLaplaceMatrix(conductivity, expectedK);

        // for destructor
        vec_eles.push_back(unitSquareQuad);
        for (auto e : vec_eles)
            for (unsigned i=0; i<e->getNNodes(true); i++)
                vec_nodes.push_back(e->getNode(i));
    }

    ~NumLibFemIsoQuad4Test()
    {
        for (auto itr = vec_nodes.begin(); itr!=vec_nodes.end(); ++itr )
            delete *itr;
        for (auto itr = vec_eles.begin(); itr!=vec_eles.end(); ++itr )
            delete *itr;
    }

    // Quad: square shape with a length of 1m
    MeshLib::Quad* createSquareQuad(double h)
    {
        MeshLib::Node** nodes = new MeshLib::Node*[e_nnodes];
        nodes[0] = new MeshLib::Node(0.0, 0.0, 0.0);
        nodes[1] = new MeshLib::Node(  h, 0.0, 0.0);
        nodes[2] = new MeshLib::Node(  h,   h, 0.0);
        nodes[3] = new MeshLib::Node(0.0,   h, 0.0);
        return new MeshLib::Quad(nodes);
    }

    // set an identity matrix
    template <class T_MATRIX, typename ID_TYPE=signed>
    void setIdentityMatrix(unsigned dim, T_MATRIX &m) const
    {
        for (unsigned i=0; i<dim; i++)
            for (unsigned j=0; j<dim; j++)
                m(i,j) = 0.0;
        for (unsigned i=0; i<dim; i++)
            m(i,i) = 1.0;
    }

    // copy upper triangles to lower triangles in a matrix
    template <class T_MATRIX, typename ID_TYPE=signed>
    void copyUpperToLower(const ID_TYPE dim, T_MATRIX &m) const
    {
        for (ID_TYPE i=0; i<dim; i++)
            for (ID_TYPE j=0; j<i; j++)
                m(i,j) = m(j,i);
    }

    // set an expected mass matrix for 1m x 1m
    template <class T_MATRIX, typename ID_TYPE=signed>
    void setExpectedMassMatrix(T_MATRIX &m)
    {
        // set upper triangle entries
        m(0,0) = 1.0; m(0,1) = 1./2; m(0,2) = 1./4; m(0,3) = 1./2;
        m(1,1) = 1.0; m(1,2) = 1./2; m(1,3) = 1./4;
        m(2,2) = 1.0; m(2,3) = 1./2;
        m(3,3) = 1.0;
        // make symmetric
        copyUpperToLower(4, m);
        m *= 1./9.;
    }

    // set an expected laplace matrix for 1m x 1m
    template <class T_MATRIX, typename ID_TYPE=signed>
    void setExpectedLaplaceMatrix(double k, T_MATRIX &m)
    {
        // set upper triangle entries
        m(0,0) = 4.0; m(0,1) = -1.0; m(0,2) = -2.0; m(0,3) = -1.0;
        m(1,1) = 4.0; m(1,2) = -1.0; m(1,3) = -2.0;
        m(2,2) = 4.0; m(2,3) = -1.0;
        m(3,3) = 4.0;
        // make symmetric
        copyUpperToLower(4, m);
        m *= k/6.;
    }

    static const double conductivity;
    static const double eps;
    DimMatrix D;
    NodalMatrix expectedM;
    NodalMatrix expectedK;
    typename FeQUAD4Type::IntegrationMethod integration_method;

    std::vector<const MeshLib::Node*> vec_nodes;
    std::vector<const MeshLib::Quad*> vec_eles;
    MeshLib::Quad* unitSquareQuad;

}; // NumLibFemIsoQuad4Test

template <class T_MATRIX_TYPES>
const double NumLibFemIsoQuad4Test<T_MATRIX_TYPES>::conductivity = 1e-11;

template <class T_MATRIX_TYPES>
const double NumLibFemIsoQuad4Test<T_MATRIX_TYPES>::eps = std::numeric_limits<double>::epsilon();

} // namespace

#ifdef OGS_USE_EIGEN

struct EigenFixedMatrixTypes
{
    typedef Eigen::Matrix<double, e_nnodes, e_nnodes, Eigen::RowMajor> NodalMatrixType;
    typedef Eigen::Matrix<double, e_nnodes, 1> NodalVectorType;
    typedef Eigen::Matrix<double, dim, e_nnodes, Eigen::RowMajor> DimNodalMatrixType;
    typedef Eigen::Matrix<double, dim, dim, Eigen::RowMajor> DimMatrixType;
};

struct EigenDynamicMatrixTypes
{
    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> NodalMatrixType;
    typedef NodalMatrixType DimNodalMatrixType;
    typedef NodalMatrixType DimMatrixType;
    typedef Eigen::VectorXd NodalVectorType;
};

#endif // OGS_USE_EIGEN

typedef ::testing::Types<
#ifdef OGS_USE_EIGEN
        EigenFixedMatrixTypes
        , EigenDynamicMatrixTypes
#endif
        > MatrixTypes;

TYPED_TEST_CASE(NumLibFemIsoQuad4Test, MatrixTypes);

TYPED_TEST(NumLibFemIsoQuad4Test, CheckMassMatrix)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeQUAD4Type FeQUAD4Type;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object
    FeQUAD4Type fe(*this->unitSquareQuad);

    // evaluate a mass matrix M = int{ N^T D N }dA_e
    NodalMatrix M(e_nnodes, e_nnodes);
    ShapeMatricesType shape(dim, e_nnodes);
    for (std::size_t i=0; i < this->integration_method.getNPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.template computeShapeFunctions<ShapeMatrixType::N_J>(wp.getCoords(), shape);
        M.noalias() += shape.N * shape.N.transpose() * shape.detJ * wp.getWeight();
    }

    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);
}

TYPED_TEST(NumLibFemIsoQuad4Test, CheckLaplaceMatrix)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeQUAD4Type FeQUAD4Type;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object
    FeQUAD4Type fe(*this->unitSquareQuad);

    // evaluate a Laplace matrix K = int{ dNdx^T D dNdx }dA_e
    NodalMatrix K(e_nnodes, e_nnodes);
    ShapeMatricesType shape(dim, e_nnodes);
    for (std::size_t i=0; i < this->integration_method.getNPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.template computeShapeFunctions<ShapeMatrixType::DNDX>(wp.getCoords(), shape);
        K.noalias() += shape.dNdx.transpose() * this->D * shape.dNdx * shape.detJ * wp.getWeight();
    }
    ASSERT_ARRAY_NEAR(this->expectedK.data(), K.data(), K.size(), this->eps);
}

TYPED_TEST(NumLibFemIsoQuad4Test, CheckMassLaplaceMatrices)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeQUAD4Type FeQUAD4Type;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object
    FeQUAD4Type fe(*this->unitSquareQuad);

    // evaluate both mass and laplace matrices at once
    NodalMatrix M(e_nnodes, e_nnodes);
    NodalMatrix K(e_nnodes, e_nnodes);
    ShapeMatricesType shape(dim, e_nnodes);
    for (std::size_t i=0; i < this->integration_method.getNPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.computeShapeFunctions(wp.getCoords(), shape);
        M.noalias() += shape.N * shape.N.transpose() * shape.detJ * wp.getWeight();
        K.noalias() += shape.dNdx.transpose() * this->D * shape.dNdx * shape.detJ * wp.getWeight();
    }
    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->expectedK.data(), K.data(), K.size(), this->eps);
}

TYPED_TEST(NumLibFemIsoQuad4Test, CheckGaussIntegrationLevel)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeQUAD4Type FeQUAD4Type;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object with gauss quadrature level 2
    FeQUAD4Type fe(*this->unitSquareQuad);

    // evaluate a mass matrix
    NodalMatrix M(e_nnodes, e_nnodes);
    ShapeMatricesType shape(dim, e_nnodes);
    ASSERT_EQ(4u, this->integration_method.getNPoints());
    for (std::size_t i=0; i < this->integration_method.getNPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.computeShapeFunctions(wp.getCoords(), shape);
        M.noalias() += shape.N * shape.N.transpose() * shape.detJ * wp.getWeight();
    }
    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);

    // Change gauss quadrature level to 3
    this->integration_method.setIntegrationOrder(3);
    M *= .0;
    ASSERT_EQ(9u, this->integration_method.getNPoints());
    for (std::size_t i=0; i < this->integration_method.getNPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.computeShapeFunctions(wp.getCoords(), shape);
        M.noalias() += shape.N * shape.N.transpose() * shape.detJ * wp.getWeight();
    }
    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);
}


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <string>
#include <set>
#include <unordered_map>
#include <vector>

#include <boost/variant.hpp>

#include "support/pn/types.hh"
#include "support/properties/formulae.hh"

namespace pnmc { namespace mc { namespace classic {

/*------------------------------------------------------------------------------------------------*/

struct integer_constant
{
  int value;
};
struct integer_sum;
struct integer_product;
struct integer_difference;
struct integer_division;
struct integer_tokens
{
  std::size_t pos;
};

using integer_ast = boost::variant< integer_constant
                                  , boost::recursive_wrapper<integer_sum>
                                  , boost::recursive_wrapper<integer_product>
                                  , boost::recursive_wrapper<integer_difference>
                                  , boost::recursive_wrapper<integer_division>
                                  , integer_tokens>;

struct integer_sum
{
  std::vector<integer_ast> expressions;
};

struct integer_product
{
  std::vector<integer_ast> expressions;
};

struct integer_difference
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_division
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

/*------------------------------------------------------------------------------------------------*/

struct invariant;
struct impossibility;
struct possibility;

struct true_ {};
struct false_ {};
struct negation;
struct conjunction;
struct disjunction;
struct exclusive_disjonction;
struct implication;
struct equivalence;

struct integer_eq;
struct integer_ne;
struct integer_lt;
struct integer_le;
struct integer_gt;
struct integer_ge;

using boolean_ast = boost::variant< boost::recursive_wrapper<invariant>
                                  , boost::recursive_wrapper<impossibility>
                                  , boost::recursive_wrapper<possibility>
                                  , true_
                                  , false_
                                  , boost::recursive_wrapper<negation>
                                  , boost::recursive_wrapper<conjunction>
                                  , boost::recursive_wrapper<disjunction>
                                  , boost::recursive_wrapper<exclusive_disjonction>
                                  , boost::recursive_wrapper<implication>
                                  , boost::recursive_wrapper<equivalence>
                                  , boost::recursive_wrapper<integer_eq>
                                  , boost::recursive_wrapper<integer_ne>
                                  , boost::recursive_wrapper<integer_lt>
                                  , boost::recursive_wrapper<integer_le>
                                  , boost::recursive_wrapper<integer_gt>
                                  , boost::recursive_wrapper<integer_ge>>;

struct invariant
{
  boolean_ast expression;
};

struct impossibility
{
  boolean_ast expression;
};

struct possibility
{
  boolean_ast expression;
};

struct negation
{
  boolean_ast expression;
};

struct conjunction
{
  std::vector<boolean_ast> expressions;
};

struct disjunction
{
  std::vector<boolean_ast> expressions;
};

struct exclusive_disjonction
{
  std::vector<boolean_ast> expressions;
};

struct implication
{
  boolean_ast lhs_expression;
  boolean_ast rhs_expression;
};

struct equivalence
{
  boolean_ast lhs_expression;
  boolean_ast rhs_expression;
};

struct integer_eq
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_ne
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_lt
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_le
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_gt
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

struct integer_ge
{
  integer_ast lhs_expression;
  integer_ast rhs_expression;
};

/*------------------------------------------------------------------------------------------------*/

integer_ast
make_ast( const properties::integer_expression&
        , const std::unordered_map<std::string, pn::valuation_type>& bounds);

/*------------------------------------------------------------------------------------------------*/

boolean_ast
make_ast( const properties::boolean_expression&, bool has_deadlock
        , const std::set<std::string>& dead_transitions
        , const std::unordered_map<std::string, pn::valuation_type>& bounds);

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::classic


#ifndef KALMAN_FILTER_MULTIARM_BANDIT_HPP
#define KALMAN_FILTER_MULTIARM_BANDIT_HPP

#include <assert.h>
#include <vector>
#include <random>
#include <utility>

#include <Eigen/Eigenvalues>

#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/eigen_helpers.hpp>

namespace smmap
{
    template <typename Generator = std::mt19937_64>
    class KalmanFilterMANB
    {
        public:
            KalmanFilterMANB(
                    const Eigen::VectorXd& prior_mean = Eigen::VectorXd::Zero(1),
                    const Eigen::VectorXd& prior_var = Eigen::VectorXd::Ones(1))
                : num_bandits_(prior_mean.rows())
                , arm_mean_(prior_mean)
                , arm_var_(prior_var)
            {
                assert(arm_mean_.cols() == arm_var_.cols());
            }

            /**
             * @brief selectArmToPull Perform Thompson sampling on the bandits,
             *                        and select the bandit with the largest sample.
             * @param generator
             * @return
             */
            ssize_t selectArmToPull(Generator& generator)
            {
                // Sample from the current distribuition
                std::normal_distribution<double> normal_dist(0.0, 1.0);

                ssize_t best_arm = -1;
                double best_sample = -std::numeric_limits<double>::infinity();
                for (ssize_t arm_ind = 0; arm_ind < num_bandits_; arm_ind++)
                {
                    const double sample = std::sqrt(arm_var_(arm_ind)) * normal_dist(generator) + arm_mean_(arm_ind);

                    if (sample > best_sample)
                    {
                        best_arm = arm_ind;
                        best_sample = sample;
                    }
                }

                assert(best_arm >= 0);
                return best_arm;
            }

            bool generateAllModelActions() const
            {
                return false;
            }

            /**
             * @brief updateArms
             * @param transition_variance
             * @param arm_pulled
             * @param observed_reward
             * @param observation_variance
             */
            void updateArms(
                    const Eigen::VectorXd& transition_variance,
                    const ssize_t arm_pulled,
                    const double observed_reward,
                    const double observation_variance)
            {
                for (ssize_t arm_ind = 0; arm_ind < num_bandits_; arm_ind++)
                {
                    if (arm_ind != arm_pulled)
                    {
                        arm_var_(arm_ind) += transition_variance(arm_ind);
                    }
                    else
                    {
                        arm_mean_(arm_ind) = ((arm_var_(arm_ind) + transition_variance(arm_ind)) * observed_reward + observation_variance * arm_mean_(arm_ind))
                                            / (arm_var_(arm_ind) + transition_variance(arm_ind) + observation_variance);

                        arm_var_(arm_ind) = (arm_var_(arm_ind) + transition_variance(arm_ind)) * observation_variance
                                           / (arm_var_(arm_ind) + transition_variance(arm_ind) + observation_variance);
                    }
                }
            }

            const Eigen::VectorXd& getMean() const
            {
                return arm_mean_;
            }

            Eigen::VectorXd getMean()
            {
                return arm_mean_;
            }

            const Eigen::VectorXd& getVariance() const
            {
                return arm_var_;
            }

            Eigen::VectorXd getVariance()
            {
                return arm_var_;
            }

        private:
            ssize_t num_bandits_;

            Eigen::VectorXd arm_mean_;
            Eigen::VectorXd arm_var_;
    };

    template<typename Generator = std::mt19937_64>
    class KalmanFilterMANDB
    {
        public:
            KalmanFilterMANDB(
                    const Eigen::VectorXd& prior_mean = Eigen::VectorXd::Ones(1),
                    const Eigen::MatrixXd& prior_covar = Eigen::MatrixXd::Identity(1, 1))
                : arm_mean_(prior_mean)
                , arm_covar_(prior_covar)
            {
                assert(arm_covar_.rows() == arm_covar_.cols());
                assert(arm_covar_.rows() == arm_mean_.rows());
            }

            /**
             * @brief selectArmToPull Perform Thompson sampling on the bandits,
             *                        and select the bandit with the largest sample.
             * @param generator
             * @return
             */
            ssize_t selectArmToPull(Generator& generator)
            {
                // Sample from the current distribuition
                arc_helpers::MultivariteGaussianDistribution distribution(arm_mean_, arm_covar_);
                const Eigen::VectorXd sample = distribution(generator);

                // Find the arm with the highest sample
                ssize_t best_arm = -1;
                sample.maxCoeff(&best_arm);

                return best_arm;
            }

            bool generateAllModelActions() const
            {
                return true;
            }

            /**
             * @brief updateArms
             * @param transition_covariance
             * @param arm_pulled
             * @param obs_reward
             * @param obs_var
             */
            void updateArms(
                    const Eigen::MatrixXd& transition_covariance,
                    const Eigen::MatrixXd& observation_matrix,
                    const Eigen::VectorXd& observed_reward,
                    const Eigen::MatrixXd& observation_covariance)
            {
                #pragma GCC diagnostic push
                #pragma GCC diagnostic ignored "-Wconversion"
                const Eigen::MatrixXd& C = observation_matrix;

                // Kalman predict
                const Eigen::VectorXd& predicted_mean = arm_mean_;                      // No change to mean
                const auto predicted_covariance = arm_covar_ + transition_covariance;   // Add process noise

                // Kalman update - symbols from wikipedia article
                const auto innovation = observed_reward - C * predicted_mean;                                            // tilde y_k
                const auto innovation_covariance = C * predicted_covariance.selfadjointView<Eigen::Lower>() * C.transpose() + observation_covariance;    // S_k
                const auto kalman_gain = predicted_covariance.selfadjointView<Eigen::Lower>() * C.transpose() * innovation_covariance.inverse();         // K_k

                arm_mean_ = predicted_mean + kalman_gain * innovation;                                                              // hat x_k|k
                arm_covar_ = predicted_covariance - kalman_gain * C * predicted_covariance.selfadjointView<Eigen::Lower>();         // P_k|k
                #pragma GCC diagnostic pop

                // Numerical problems fixing
                arm_covar_ = ((arm_covar_ + arm_covar_.transpose()) * 0.5).selfadjointView<Eigen::Lower>();

                assert(!(arm_mean_.unaryExpr([] (const double &val) { return std::isnan(val); })).any() && "NaN Found in arm_mean_ in kalman banidt!");
                assert(!(arm_mean_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "Inf Found in arm_mean_ in kalman banidt!");
                assert(!(arm_covar_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "NaN Found in arm_covar_ in kalman bandit!");
                assert(!(arm_covar_.unaryExpr([] (const double &val) { return std::isinf(val); })).any() && "Inf Found in arm_covar_ in kalman bandit!");
            }

            const Eigen::VectorXd& getMean() const
            {
                return arm_mean_;
            }

            Eigen::VectorXd getMean()
            {
                return arm_mean_;
            }

            const Eigen::MatrixXd& getCovariance() const
            {
                return arm_covar_;
            }

            Eigen::MatrixXd getCovariance()
            {
                return arm_covar_;
            }

        private:
            Eigen::VectorXd arm_mean_;
            Eigen::MatrixXd arm_covar_;
    };
}

#endif // KALMAN_FILTER_MULTIARM_BANDIT_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TENPOWER_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/any.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>
#include <boost/simd/function/simd/is_odd.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/rec.hpp>
#include <boost/simd/function/simd/shift_right.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(tenpower_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bd::int_<A0>, X>
                             )
   {
      using result = bd::as_floating_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        result res = One<result>();
        result base = Ten<result>();
        A0 exp = bs::abs(a0);
        while(any(exp))
        {
          //       res *= if_else(is_odd(exp), base, One<result>()); TO DO
          res =  res * if_else(is_odd(exp), base, One<result>());
          //  exp >>= 1; TODO
          exp =  shift_right(exp, 1);
          base = sqr(base);
        }
        return if_else(is_ltz(a0), bs::rec(res), res);
      }
   };

   BOOST_DISPATCH_OVERLOAD(tenpower_
                             , (typename A0, typename X)
                             , bd::cpu_
                             , bs::pack_<bd::uint_<A0>, X>
                             )
   {
      using result = bd::as_floating_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        result res = One<result>();
        result base = Ten<result>();
        A0 exp = a0;
        while(any(exp))
        {
          res = res*if_else(is_odd(exp), base, One<result>()); // TODO
//          res *= if_else(is_odd(exp), base, One<result>());
          //  exp >>= 1; TODO
          exp =  shift_right(exp, 1);
          base = sqr(base);
        }
        return res;
      }
   };

} } }

#endif


#ifndef MATH_DUAL_HPP
#define MATH_DUAL_HPP

#ifndef _E_of
#define _E_of(X) X "E"
//#define _E_of(X) X "\u0190"
#endif

#include <boost/operators.hpp>

namespace Math {
	template<typename>
		struct quat;

	template<typename R = float>
	struct dual: public boost::operators<dual<R>> {
		quat<R> u, v;

		/** Additive inverse */
		dual<R> operator-(void) const;
		/** Multiplicative inverse */
		dual<R> operator!(void) const;
		/** Distributes conjugation to members */
		dual<R> operator~(void) const;
		/** Cast operator, as Euclidean norm */
		explicit operator R(void) const;
		/** Squared Euclidean norm */
		R operator()(void) const;
		/** Distributes equality test */
		bool operator==(dual<R> const &rhs) const;
		/** Apply (lhs * rhs * ~lhs) */
		dual<R> operator()(quat<R> const &rhs) const;
		dual<R> operator()(dual<R> const &rhs) const;

		dual<R>& operator=(quat<R> const &rhs) {
			u = rhs; v = 0;
			return *this;
		}
		dual<R>& operator=(dual<R> const &rhs) = default;
		dual<R>& operator+=(R const &rhs) {
			u.w += rhs;
			return *this;
		}
		dual<R>& operator+=(dual<R> const &rhs) {
			u += rhs.u;
			v += rhs.v;
			return *this;
		}
		dual<R>& operator-=(dual<R> const &rhs) {
			u -= rhs.u;
			v -= rhs.v;
			return *this;
		}
		dual<R>& operator*=(dual<R> const &rhs) {
			auto const& p = rhs.u, q = rhs.v;
 			auto const r = u, s = v;
			return *this = {
				r.w*p.w - r.x*p.x - r.y*p.y - r.z*p.z,
				r.w*p.x + r.x*p.w + r.y*p.z - r.z*p.y,
				r.w*p.y - r.x*p.z + r.y*p.w + r.z*p.x,
				r.w*p.z + r.x*p.y - r.y*p.x + r.z*p.w,
				s.w*p.w - s.x*p.x - s.y*p.y - s.z*p.z
					+ r.w*q.w - r.x*q.x - r.y*q.y - r.z*q.z,
				s.x*p.w + s.w*p.x - s.z*p.y + s.y*p.z
					+ r.x*q.w + r.w*q.x - r.z*q.y + r.y*q.z,
				s.y*p.w + s.z*p.x + s.w*p.y - s.x*p.z
					+ r.y*q.w + r.z*q.x + r.w*q.y - r.x*q.z,
				s.z*p.w - s.y*p.x + s.x*p.y + s.w*p.z
					+ r.z*q.w - r.y*q.x + r.x*q.y + r.w*q.z
			};
		}
		dual<R>& operator*=(quat<R> const &rhs) {
			u *= rhs;
			v *= rhs;
			return *this;
		}
		dual<R>& operator*=(R const &rhs) {
			u *= rhs;
			v *= rhs;
			return *this;
		}
		dual<R>& operator/=(R const &rhs) {
			u /= rhs;
			v /= rhs;
			return *this;
		}

		dual(void) = default;
		dual(dual<R> const&) = default;
		dual(dual<R> &&) = default;
		dual(R uw, R ux = 0, R uy = 0, R uz = 0,
				R vw = 0, R vx = 0, R vy = 0, R vz = 0):
			u(uw, ux, uy, uz), v(vw, vx, vy, vz) {}
		dual(const quat<R> &u, const quat<R> v = {0}):
			u(u), v(v) {}
	};

	template<typename R>
	dual<R> dual<R>::operator-(void) const {
		return {-u, -v};
	}
	template<typename R>
	dual<R> dual<R>::operator!(void) const {
		dual<R> conj = ~*this;
		return conj/conj();
	}
	template<typename R>
	dual<R> dual<R>::operator~(void) const {
		return {{u.w,-u.x,-u.y,-u.z},
			{-v.w, v.x, v.y, v.z}};
	}
	template<typename R>
	dual<R>::operator R(void) const {
		return sqrt((*this)());
	}
	template<typename R>
	R dual<R>::operator()(void) const {
		return u()+v();
	}

	template<typename R>
	bool dual<R>::operator==(dual<R> const& rhs) const {
		return u == rhs.u && v == rhs.v;
	}
	template<typename R>
	dual<R> dual<R>::operator()(quat<R> const& rhs) const {
		return *this * rhs * ~*this;
	}
	template<typename R>
	dual<R> dual<R>::operator()(dual<R> const& rhs) const {
		return *this * rhs * ~*this;
	}
}

#endif


// Base-2 logarithm bithack.

#ifndef _AFJDFJSDFSD_PYCUDA_HEADER_SEEN_BITLOG_HPP
#define _AFJDFJSDFSD_PYCUDA_HEADER_SEEN_BITLOG_HPP

#include <climits>
#include <boost/cstdint.hpp>

namespace pyhip
{
  extern const char log_table_8[];

  inline unsigned bitlog2_16(boost::uint16_t v)
  {
    if (unsigned long t = v >> 8)
      return 8+log_table_8[t];
    else
      return log_table_8[v];
  }

  inline unsigned bitlog2_32(boost::uint32_t v)
  {
    if (boost::uint16_t t = v >> 16)
      return 16+bitlog2_16(t);
    else
      return bitlog2_16(boost::uint16_t(v));
  }

  inline unsigned bitlog2(size_t v)
  {
#if (ULONG_MAX != 4294967295) || defined(_WIN64)
    if (boost::uint32_t t = v >> 32)
      return 32+bitlog2_32(t);
    else
#endif
      return bitlog2_32(unsigned(v));
   }
}

#endif


#include <CGAL/Cartesian.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_sqrt.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Periodic_2_Delaunay_triangulation_traits_2.h>
#include <CGAL/Periodic_2_Delaunay_triangulation_2.h>
#include <CGAL/determinant.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Timer.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>           Traits;
typedef Traits::FT                                                              NT;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>            Triangulation;
typedef CGAL::Hyperbolic_octagon_translation_matrix<NT>                         Octagon_matrix;
typedef Triangulation::Point                                                    Point;
typedef Triangulation::Vertex_handle                                            Vertex_handle;
typedef Traits::Side_of_original_octagon                                        Side_of_original_octagon;

typedef CGAL::Cartesian<double>::Point_2                                        Point_double;
typedef CGAL::Creator_uniform_2<double, Point_double >                          Creator;

typedef CGAL::Exact_predicates_exact_constructions_kernel_with_sqrt             EKernel;
typedef CGAL::Delaunay_triangulation_2<EKernel>                                 Euclidean_triangulation;
typedef CGAL::Periodic_2_Delaunay_triangulation_traits_2<EKernel>               Ptraits;
typedef CGAL::Periodic_2_Delaunay_triangulation_2<Ptraits>                      PEuclidean_triangulation;

typedef double                                                                  dNT;
typedef CGAL::Cartesian<dNT>                                                    dKernel;
typedef CGAL::Delaunay_triangulation_2<dKernel>                                 dTriangulation;

int main(int argc, char** argv)
{
  int N, iters;
  iters = 1;
  if(argc < 2)
  {
    std::cout << "usage: " << argv[0] << " [number_of_points_to_insert] [optional: number_of_iterations]" << std::endl;
    std::cout << "Defaulting to values: 10000000, 10..." << std::endl;
    N = 1000000;
    iters = 10;
  } else {
    N = atoi(argv[1]);
    if (argc < 3)
      iters = 1;
    else
      iters = atoi(argv[2]);
  }

  Side_of_original_octagon pred;

  std::cout << "---- for best results, make sure that you have compiled me in Release mode ----" << std::endl;

  double extime1 = 0.0;
  double extime2 = 0.0;
  double extime3 = 0.0;

  for(int exec = 1; exec <= iters; ++exec)
  {
    std::vector<Point> pts;
    std::vector<Point_double> dpts;
    CGAL::Random_points_in_disc_2<Point_double, Creator> g(0.85);

    int cnt = 0;
    std::cout << "================ iteration " << exec << " : generating points ================" << std::endl;
    do
    {
      Point_double pd = *(++g);
      Point pt = Point(pd.x(), pd.y());
      if(pred(pt) != CGAL::ON_UNBOUNDED_SIDE)
      {
        pts.push_back(pt);
        dpts.push_back(pd);
        ++cnt;
      }
    }
    while(cnt < N);

    if(cnt < N)
    {
      std::cout << "Failed to generate all the random points! Exiting..." << std::endl;
      return -1;
    }

    std::cout << "Inserting into hyperbolic periodic  CORE  triangulation...    "; std::cout.flush();
    Triangulation tr;
    CGAL::Timer t1;
    t1.start();
    tr.insert(pts.begin(), pts.end());
    t1.stop();
    extime1 += t1.time();
    std::cout << "DONE! (# of vertices = " << tr.number_of_vertices() << ", time = " << t1.time() << " secs)" << std::endl;

    std::cout << "inserting into Euclidean non-periodic  CORE  triangulation... "; std::cout.flush();
    Euclidean_triangulation etr;
    CGAL::Timer t2;
    t2.start();
    etr.insert(pts.begin(), pts.end());
    t2.stop();
    extime2 += t2.time();
    std::cout << "DONE! (# of vertices = " << etr.number_of_vertices() << ", time = " << t2.time() << " secs)" << std::endl;

    std::cout << "Inserting into Euclidean non-periodic DOUBLE triangulation... "; std::cout.flush();
    dTriangulation dtr;
    CGAL::Timer t3;
    t3.start();
    dtr.insert(dpts.begin(), dpts.end());
    t3.stop();
    extime3 += t3.time();
    std::cout << "DONE! (# of vertices = " << dtr.number_of_vertices() << ", time = " << t3.time() << " secs)" << std::endl;
  }

  double diters(iters);
  extime1 /= diters;
  extime2 /= diters;
  extime3 /= diters;

  std::cout << "Hyperbolic periodic      CORE  triangulation: average time = " << extime1 << std::endl;
  std::cout << "Euclidean  non-periodic  CORE  triangulation: average time = " << extime2 << std::endl;
  std::cout << "Euclidean  non-periodic DOUBLE triangulation: average time = " << extime3 << std::endl;

  return EXIT_SUCCESS;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <ql/experimental/math/convolvedstudentt.hpp>
#include <ql/errors.hpp>
#include <ql/math/factorial.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/functional.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/math/distributions/students_t.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {

    CumulativeBehrensFisher::CumulativeBehrensFisher(const std::vector<Integer>& degreesFreedom,
                                                     const std::vector<Real>& factors)
    : degreesFreedom_(degreesFreedom), factors_(factors), polyConvolved_(std::vector<Real>(1, 1.))

    {
        QL_REQUIRE(degreesFreedom.size() == factors.size(),
            "Incompatible sizes in convolution.");
        for (int i : degreesFreedom) {
            QL_REQUIRE(i % 2 != 0, "Even degree of freedom not allowed");
            QL_REQUIRE(i >= 0, "Negative degree of freedom not allowed");
        }
        for(Size i=0; i<degreesFreedom_.size(); i++)
            polynCharFnc_.push_back(polynCharactT((degreesFreedom[i]-1)/2));
        // adjust the polynomial coefficients by the factors in the linear
        //   combination:
        for(Size i=0; i<degreesFreedom_.size(); i++) {
            Real multiplier = 1.;
            for(Size k=1; k<polynCharFnc_[i].size(); k++) {
                multiplier *= std::abs(factors_[i]);
                polynCharFnc_[i][k] *= multiplier;
            }
        }
        //convolution, here it is a product of polynomials and exponentials
        for (auto& i : polynCharFnc_)
            polyConvolved_ = convolveVectorPolynomials(polyConvolved_, i);
        // trim possible zeros that might have arised:
        auto it = polyConvolved_.rbegin();
        while (it != polyConvolved_.rend()) {
            if (*it == 0.) {
                polyConvolved_.pop_back();
                it = polyConvolved_.rbegin();
              }else{
                  break;
              }
          }
          // cache 'a' value (the exponent)
          for(Size i=0; i<degreesFreedom_.size(); i++)
              a_ += std::sqrt(static_cast<Real>(degreesFreedom_[i]))
                * std::abs(factors_[i]);
          a2_ = a_ * a_;
    }

    std::vector<Real> CumulativeBehrensFisher::polynCharactT(Natural n) const {
        Natural nu = 2 * n +1;
        std::vector<Real> low(1,1.), high(1,1.);
        high.push_back(std::sqrt(static_cast<Real>(nu)));
        if(n==0) return low;
        if(n==1) return high;

        for(Size k=1; k<n; k++) {
            std::vector<Real> recursionFactor(1,0.); // 0 coef
            recursionFactor.push_back(0.); // 1 coef
            recursionFactor.push_back(nu/((2.*k+1.)*(2.*k-1.))); // 2 coef
            std::vector<Real> lowUp =
                convolveVectorPolynomials(recursionFactor, low);
            //add them up:
            for(Size i=0; i<high.size(); i++)
                lowUp[i] += high[i];
            low = high;
            high = lowUp;
        }
        return high;
    }

    std::vector<Real> CumulativeBehrensFisher::convolveVectorPolynomials(
        const std::vector<Real>& v1,
        const std::vector<Real>& v2) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(!v1.empty() && !v2.empty(),
            "Incorrect vectors in polynomial.");
    #endif

        const std::vector<Real>& shorter = v1.size() < v2.size() ? v1 : v2;
        const std::vector<Real>& longer = (v1 == shorter) ? v2 : v1;

        Size newDegree = v1.size()+v2.size()-2;
        std::vector<Real> resultB(newDegree+1, 0.);
        for(Size polyOrdr=0; polyOrdr<resultB.size(); polyOrdr++) {
            for(Size i=std::max<Integer>(0, polyOrdr-longer.size()+1);
                i<=std::min(polyOrdr, shorter.size()-1); i++)
                resultB[polyOrdr] += shorter[i]*longer[polyOrdr-i];
        }
        return resultB;
    }

    Probability CumulativeBehrensFisher::operator()(const Real x) const {
        // 1st & 0th terms with the table integration
        Real integral = polyConvolved_[0] * std::atan(x/a_);
        Real squared = a2_ + x*x;
        Real rootsqr = std::sqrt(squared);
        Real atan2xa = std::atan2(-x,a_);
        if(polyConvolved_.size()>1)
            integral += polyConvolved_[1] * x/squared;

        for(Size exponent = 2; exponent <polyConvolved_.size(); exponent++) {
            integral -= polyConvolved_[exponent] *
                Factorial::get(exponent-1) * std::sin((exponent)*atan2xa)
                    /std::pow(rootsqr, static_cast<Real>(exponent));
         }
        return .5 + integral / M_PI;
    }

    Probability
    CumulativeBehrensFisher::density(const Real x) const {
        Real squared = a2_ + x*x;
        Real integral = polyConvolved_[0] * a_ / squared;
        Real rootsqr = std::sqrt(squared);
        Real atan2xa = std::atan2(-x,a_);
        for(Size exponent=1; exponent <polyConvolved_.size(); exponent++) {
            integral += polyConvolved_[exponent] *
                Factorial::get(exponent) * std::cos((exponent+1)*atan2xa)
                    /std::pow(rootsqr, static_cast<Real>(exponent+1) );
        }
        return integral / M_PI;
    }

    InverseCumulativeBehrensFisher::InverseCumulativeBehrensFisher(
        const std::vector<Integer>& degreesFreedom,
        const std::vector<Real>& factors,
        Real accuracy)
    : normSqr_(std::inner_product(factors.begin(), factors.end(),
        factors.begin(), 0.)),
      accuracy_(accuracy), distrib_(degreesFreedom, factors) { }

    Real InverseCumulativeBehrensFisher::operator()(const Probability q) const {
        Probability effectiveq;
        Real sign;
        // since the distrib is symmetric solve only on the right side:
        if(q==0.5) {
            return 0.;
        }else if(q < 0.5) {
            sign = -1.;
            effectiveq = 1.-q;
        }else{
            sign = 1.;
            effectiveq = q;
        }
        Real xMin =
            InverseCumulativeNormal::standard_value(effectiveq) * normSqr_;
        // inversion will fail at the Brent's bounds-check if this is not enough
        // (q is very close to 1.), in a bad combination fails around 1.-1.e-7
        Real xMax = 1.e6;
        return sign *
            Brent().solve([&](Real x){ return distrib_(x) - effectiveq; },
                          accuracy_, (xMin+xMax)/2., xMin, xMax);
    }

}


#include "surveypattern.h"
#include "waypoint.h"
#include <QPainter>
#include <QtMath>
#include <QJsonObject>
#include <QJsonArray>
#include <QDebug>
#include "platform.h"
#include "autonomousvehicleproject.h"
#include "surveyarea.h"

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

namespace bg = boost::geometry;

SurveyPattern::SurveyPattern(MissionItem *parent):GeoGraphicsMissionItem(parent),
    m_startLocation(nullptr),m_endLocation(nullptr),m_spacing(1.0),m_direction(0.0),m_alignment(Alignment::start),m_spacingLocation(nullptr),m_internalUpdateFlag(false)
{
    setShowLabelFlag(true);
}

Waypoint * SurveyPattern::createWaypoint()
{
    Waypoint * wp = createMissionItem<Waypoint>();
    wp->setFlag(QGraphicsItem::ItemIsMovable);
    wp->setFlag(QGraphicsItem::ItemIsSelectable);
    wp->setFlag(QGraphicsItem::ItemSendsGeometryChanges);
    wp->setFlag(QGraphicsItem::ItemSendsScenePositionChanges);
    connect(wp, &Waypoint::waypointMoved, this, &SurveyPattern::waypointHasChanged);
    connect(wp, &Waypoint::waypointAboutToMove, this, &SurveyPattern::waypointAboutToChange);
    return wp;
}

void SurveyPattern::setStartLocation(const QGeoCoordinate &location)
{
    if(m_startLocation == nullptr)
    {
        m_startLocation = createWaypoint();
        m_startLocation->setObjectName("start");
    }
    m_startLocation->setLocation(location);
    setPos(m_startLocation->geoToPixel(location,autonomousVehicleProject()));
    m_startLocation->setPos(m_startLocation->geoToPixel(location,autonomousVehicleProject()));
    update();
}

void SurveyPattern::setEndLocation(const QGeoCoordinate &location, bool calc)
{
    if(m_endLocation == nullptr)
    {
        m_endLocation = createWaypoint();
        m_endLocation->setObjectName("end");
    }
    m_endLocation->setLocation(location);
    m_endLocation->setPos(m_endLocation->geoToPixel(location,autonomousVehicleProject()));
    if(calc)
        calculateFromWaypoints();
    update();
}

void SurveyPattern::setSpacingLocation(const QGeoCoordinate &location, bool calc)
{
    if(m_spacingLocation == nullptr)
    {
        m_spacingLocation = createWaypoint();
        m_spacingLocation->setObjectName("spacing/direction");
    }
    m_spacingLocation->setLocation(location);
    if(calc)
        calculateFromWaypoints();
    update();
}

void SurveyPattern::calculateFromWaypoints()
{
    if(m_startLocation && m_endLocation)
    {
        qreal ab_distance = m_startLocation->location().distanceTo(m_endLocation->location());
        qreal ab_angle = m_startLocation->location().azimuthTo(m_endLocation->location());

        qreal ac_distance = 1.0;
        m_spacing = ab_distance/10.0;
        qreal ac_angle = 90.0;
        if(m_spacingLocation)
        {
            ac_distance = m_startLocation->location().distanceTo(m_spacingLocation->location());
            ac_angle = m_startLocation->location().azimuthTo(m_spacingLocation->location());
            m_spacing = ac_distance;
            m_direction = ac_angle-90;
        }
        qreal leg_heading = ac_angle-90.0;
        m_lineLength = ab_distance*qCos(qDegreesToRadians(ab_angle-leg_heading));
        m_totalWidth = ab_distance*qSin(qDegreesToRadians(ab_angle-leg_heading));
    }
}

void SurveyPattern::write(QJsonObject &json) const
{
    MissionItem::write(json);
    json["type"] = "SurveyPattern";
    if(m_startLocation)
    {
        QJsonObject slObject;
        m_startLocation->write(slObject);
        json["startLocation"] = slObject;
    }
    if(m_endLocation)
    {
        QJsonObject elObject;
        m_endLocation->write(elObject);
        json["endLocation"] = elObject;
    }
    json["spacing"] = m_spacing;
    json["direction"] = m_direction;
    switch(m_alignment)
    {
        case start:
            json["alignment"] = "start";
            break;
        case center:
            json["alignment"] = "center";
            break;
        case finish:
            json["alignment"] = "finish";
            break;
    }
    QJsonArray tracklineArray;
    auto lines = getLines();
    for (auto line: lines){
        QJsonObject tracklineObject;
        tracklineObject["type"] = "TrackLine";
        QJsonArray wpArray;
        for (auto wp: line)
        {
            QJsonObject wpObject;
            wpObject["type"] = "Waypoint";
            wpObject["latitude"] = wp.latitude();
            wpObject["longitude"] = wp.longitude();
            wpArray.append(wpObject);
        }
        tracklineObject["waypoints"] = wpArray;
        tracklineArray.append(tracklineObject);
    }
    json["children"] = tracklineArray;
}

void SurveyPattern::writeToMissionPlan(QJsonArray& navArray) const
{
    auto lines = getLines();
    for(int i = 0; i < lines.size(); i++)
    {
        auto l = lines[i];
        QJsonObject navItem;
        navItem["pathtype"] = "trackline";
        AutonomousVehicleProject* avp = autonomousVehicleProject();
        if(avp)
        {
            Platform *platform = avp->currentPlatform();
            if(platform)
            {
                QJsonObject params;
                params["speed_ms"] = platform->speed()*0.514444; // knots to m/s
                navItem["parameters"] = params;
            }
        }
        writeBehaviorsToMissionPlanObject(navItem);
        QJsonArray pathNavArray;
        for(auto wp: l)
        {
            Waypoint * temp_wp = new Waypoint();
            temp_wp->setLocation(wp);
            temp_wp->writeNavToMissionPlan(pathNavArray);
            delete temp_wp;
        }
        navItem["nav"] = pathNavArray;
        navItem["type"] = "survey_line";
        navArray.append(navItem);
    }
}

void SurveyPattern::read(const QJsonObject &json)
{
    m_startLocation = createWaypoint();
    m_startLocation->read(json["startLocation"].toObject());
    m_endLocation = createWaypoint();
    m_endLocation->read(json["endLocation"].toObject());
    setDirectionAndSpacing(json["direction"].toDouble(),json["spacing"].toDouble());
    if(json.contains("alignment"))
    {
        if(json["alignment"] == "start")
            m_alignment = start;
        if(json["alignment"] == "center")
            m_alignment = center;
        if(json["alignment"] == "finish")
            m_alignment = finish;
    }
    calculateFromWaypoints();
}

bool SurveyPattern::hasSpacingLocation() const
{
    return (m_spacingLocation != nullptr);
}

double SurveyPattern::spacing() const
{
    return m_spacing;
}

double SurveyPattern::direction() const
{
    return m_direction;
}

SurveyPattern::Alignment SurveyPattern::alignment() const
{
    return m_alignment;
}

double SurveyPattern::lineLength() const
{
    return m_lineLength;
}

double SurveyPattern::totalWidth() const
{
    return m_totalWidth;
}

Waypoint * SurveyPattern::startLocationWaypoint() const
{
    return m_startLocation;
}

Waypoint * SurveyPattern::endLocationWaypoint() const
{
    return m_endLocation;
}

void SurveyPattern::setDirectionAndSpacing(double direction, double spacing)
{
    m_direction = direction;
    m_spacing = spacing;
    QGeoCoordinate c = m_startLocation->location().atDistanceAndAzimuth(spacing,direction+90.0);
    m_internalUpdateFlag = true;
    setSpacingLocation(c,false);
    m_internalUpdateFlag = false;
}

void SurveyPattern::setAlignment(SurveyPattern::Alignment alignment)
{
    prepareGeometryChange();
    m_alignment = alignment;
    qDebug() << "alignment: " << alignment;
    update();
}

void SurveyPattern::setLineLength(double lineLength)
{
    m_lineLength = lineLength;
    updateEndLocation();
}

void SurveyPattern::setTotalWidth(double totalWidth)
{
    m_totalWidth = totalWidth;
    updateEndLocation();
}

void SurveyPattern::updateEndLocation()
{
    m_internalUpdateFlag = true;
    QGeoCoordinate p = m_startLocation->location().atDistanceAndAzimuth(m_lineLength, m_direction);
    p = p.atDistanceAndAzimuth(m_totalWidth,m_direction+90.0);
    setEndLocation(p,false);
    m_internalUpdateFlag = false;
}

QRectF SurveyPattern::boundingRect() const
{
    return shape().boundingRect().marginsAdded(QMarginsF(5,5,5,5));
}

void SurveyPattern::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
    auto lines = getLines();
    if(lines.length() > 0)
    {
        painter->save();

        bool selected = false;
        if(autonomousVehicleProject()->currentSelected() == this)
            selected = true;

        QPen p;
        p.setCosmetic(true);
        if (selected)
        {
            p.setWidth(7);
            p.setColor(Qt::white);
            painter->setPen(p);

            for (auto l:lines)
            {
                auto first = l.begin();
                auto second = first;
                second++;
                while(second != l.end())
                {
                    p.setWidth(10);
                    p.setColor(Qt::blue);
                    painter->setPen(p);
                    painter->drawPoint(m_startLocation->geoToPixel(*first,autonomousVehicleProject()));
                    p.setWidth(8);
                    p.setColor(Qt::black);
                    painter->setPen(p);
                    painter->drawLine(m_startLocation->geoToPixel(*first,autonomousVehicleProject()),m_startLocation->geoToPixel(*second,autonomousVehicleProject()));

                    first++;
                    second++;
                }
                p.setWidth(10);
                p.setColor(Qt::blue);
                painter->setPen(p);
                painter->drawPoint(m_startLocation->geoToPixel(*first,autonomousVehicleProject()));
            }
        }
        if(locked())
            p.setColor(m_lockedColor);
        else
            p.setColor(m_unlockedColor);
        p.setWidth(3);
        painter->setPen(p);

        bool turn = true;
        for (auto l:lines)
        {
            turn = !turn;
            auto first = l.begin();
            auto second = first;
            second++;
            while(second != l.end())
            {
                p.setWidth(10);
                p.setColor(Qt::blue);
                painter->setPen(p);
                painter->drawPoint(m_startLocation->geoToPixel(*first,autonomousVehicleProject()));
                if (selected)
                    p.setWidth(5);
                else
                    p.setWidth(3);
                if(locked())
                    p.setColor(m_lockedColor);
                else
                    p.setColor(m_unlockedColor);
                painter->setPen(p);
                painter->drawLine(m_startLocation->geoToPixel(*first,autonomousVehicleProject()),m_startLocation->geoToPixel(*second,autonomousVehicleProject()));

//                 if(!turn || m_arcCount < 2)
//                 {
//                     QPainterPath ret(m_startLocation->geoToPixel(*first,autonomousVehicleProject()));
//                     drawArrow(ret,m_startLocation->geoToPixel(*second,autonomousVehicleProject()),m_startLocation->geoToPixel(*first,autonomousVehicleProject()));
//                     painter->drawPath(ret);
//                 }

                first++;
                second++;
            }
            p.setWidth(10);
            p.setColor(Qt::blue);
            painter->setPen(p);
            painter->drawPoint(m_startLocation->geoToPixel(*first,autonomousVehicleProject()));
        }
        painter->restore();
    }
    return;

}

QPainterPath SurveyPattern::shape() const
{
    auto lines = getLines();
    if(!lines.empty())
    {
        if(!lines.front().empty())
        {
            QPainterPath ret(m_startLocation->geoToPixel(lines.front().front(),autonomousVehicleProject()));
            for(auto l: lines)
                for(auto p:l)
                    ret.lineTo(m_startLocation->geoToPixel(p,autonomousVehicleProject()));
            QPainterPathStroker pps;
            pps.setWidth(10);
            return pps.createStroke(ret);
        }
    }
    return QPainterPath();
}

QList<QList<QGeoCoordinate> > SurveyPattern::getLines() const
{
    QList<QList<QGeoCoordinate> > ret;
    if(m_startLocation && m_endLocation)
    {

        qreal diagonal_distance = m_startLocation->location().distanceTo(m_endLocation->location());
        qreal diagonal_angle = m_startLocation->location().azimuthTo(m_endLocation->location());

        qreal line_spacing = 1.0;
        qreal spacing_angle = 90.0;
        if(m_spacingLocation)
        {
            line_spacing = m_startLocation->location().distanceTo(m_spacingLocation->location());
            spacing_angle = m_startLocation->location().azimuthTo(m_spacingLocation->location());
        }
        else
            line_spacing = diagonal_distance/10.0;

        qreal leg_heading = spacing_angle-90.0;
        qreal leg_length = diagonal_distance*qCos(qDegreesToRadians(diagonal_angle-leg_heading));

        qreal surveyWidth = diagonal_distance*qSin(qDegreesToRadians(diagonal_angle-leg_heading));

        int line_count = qCeil(surveyWidth/line_spacing);

        qreal residual_distance = surveyWidth - ((line_count-1)*line_spacing);

        QList<QGeoCoordinate> line;
        line.append(m_startLocation->location().atDistanceAndAzimuth(m_alignment*residual_distance/2.0,spacing_angle));
        line.append(line.back().atDistanceAndAzimuth(leg_length,leg_heading));
        ret.append(line);

        for (int i = 1; i < line_count; i++)
        {
            line = QList<QGeoCoordinate>();
            line.append(ret.back().back().atDistanceAndAzimuth(line_spacing,spacing_angle));
            line.append(ret.back().front().atDistanceAndAzimuth(line_spacing,spacing_angle));
            ret.append(line);
        }

        // Check if we are a child of a SurveyArea.
        SurveyArea * surveyAreaParent = qobject_cast<SurveyArea*>(parent());
        if(surveyAreaParent)
        {
            typedef bg::model::d2::point_xy<double> BPoint;
            typedef bg::model::multi_point<BPoint> BMultiPoint;
            typedef bg::model::linestring<BPoint> BLineString;
            typedef bg::model::polygon<BPoint> BPolygon;
            typedef bg::model::multi_linestring<BLineString> BMultiLineString;

            BPolygon area_poly;

            for(auto wp: surveyAreaParent->waypoints())
            {
                BPoint p(wp->location().latitude(), wp->location().longitude());
                area_poly.outer().push_back(p);
            }

            bg::correct(area_poly);

            QList<QList<QGeoCoordinate> > clipped_ret;
            for(auto line: ret)
            {
                BPoint p1(line.front().latitude(),line.front().longitude());
                BPoint p2(line.back().latitude(),line.back().longitude());
                BLineString bline;
                bline.push_back(p1);
                bline.push_back(p2);

                BMultiLineString mls;
                bg::intersection(bline, area_poly, mls);

                for(auto l: mls)
                {
                    QList<QGeoCoordinate> clipped_line;
                    for(auto p: l)
                        clipped_line.push_back(QGeoCoordinate(p.x(), p.y()));
                    clipped_ret.push_back(clipped_line);
                }
            }

            return clipped_ret;
        }
    }
    return ret;
}

void SurveyPattern::waypointAboutToChange()
{
    prepareGeometryChange();
}

void SurveyPattern::waypointHasChanged(Waypoint *wp)
{
    if(!m_internalUpdateFlag)
        calculateFromWaypoints();
    updateETE();
    emit surveyPatternUpdated();
}

void SurveyPattern::updateProjectedPoints()
{
    if(m_startLocation)
        m_startLocation->updateProjectedPoints();
    if(m_endLocation)
        m_endLocation->updateProjectedPoints();
    if(m_spacingLocation)
        m_spacingLocation->updateProjectedPoints();
}

void SurveyPattern::reverseDirection()
{
    prepareGeometryChange();

    auto direction = m_direction;
    auto spacing = m_spacing;

    auto l = m_startLocation->location();

    m_startLocation->setLocation(m_endLocation->location());
    m_endLocation->setLocation(l);

    setDirectionAndSpacing(direction+180,spacing);
    calculateFromWaypoints();

    if(m_alignment == start)
        m_alignment = finish;
    else if(m_alignment == finish)
        m_alignment = start;

    emit surveyPatternUpdated();

    update();
}