#pragma once
#include "macro.hpp"
#include "myintrin.hpp"
#include <boost/serialization/access.hpp>
#include <boost/serialization/traits.hpp>
#include <boost/serialization/nvp.hpp>
#include <functional>

#if !defined(SSE_LEVEL) || SSE_LEVEL <= 2
	#define SUMVEC(r)	{ reg128 tmp = reg_shuffle_ps(r, r, _REG_SHUFFLE(0,1,2,3)); \
		tmp = reg_add_ps(r, tmp); \
		r = reg_shuffle_ps(tmp,tmp, _REG_SHUFFLE(1,0,0,1)); \
		r = reg_add_ps(r, tmp); }
#else
	// rの各要素を足し合わせる
	#define SUMVEC(r)	{ r = reg_hadd_ps(r,r); \
		r = reg_hadd_ps(r,r); }
#endif
#define RCP22BIT(r) { reg128 tmprcp = reg_rcp_ps(r); \
	r = reg_mul_ps(r, reg_mul_ps(tmprcp, tmprcp)); \
	tmprcp = reg_add_ps(tmprcp,tmprcp); \
	r = reg_sub_ps(tmprcp, r); }

#include <cmath>
#include <cstdint>
namespace spn {
//! ニュートン法で逆数を計算
inline reg128 Rcp22Bit(reg128 r) {
	reg128 tmp(reg_rcp_ps(r));
	r = reg_mul_ps(r, reg_mul_ps(tmp,tmp));
	tmp = reg_add_ps(tmp,tmp);
	return reg_sub_ps(tmp, r);
}
//! ニュートン法で逆数を計算して積算 = reg_div_psよりはマシな除算
inline reg128 _mmDivPs(reg128 r0, reg128 r1) {
	return reg_mul_ps(r0, Rcp22Bit(r1));
}
inline reg128 _mmAbsPs(reg128 r) {
	const static reg128 signMask = reg_set1_ps(-0.0f);
	return reg_andnot_ps(signMask, r);
}
inline reg128 _mmSetPs(float w, float z, float y, float x) {
	const float tmp[4]{w,z,y,x};
	return reg_loadu_ps(tmp);
}
inline reg128 _mmSetPs(float s) {
	return reg_load1_ps(&s);
}
inline reg128 _mmSetPdw(int32_t w, int32_t z, int32_t y, int32_t x) {
	const int32_t tmp[4] = {w,z,y,x};
	return reg_loadu_ps(reinterpret_cast<const float*>(tmp));
}
inline reg128 _mmSetPdw(int32_t d) {
	return _mmSetPdw(d,d,d,d);
}

inline float Rcp22Bit(float s) {
	reg128 tmp = reg_load_ss(&s);
	RCP22BIT(tmp)
	float ret;
	reg_store_ss(&ret, tmp);
	return ret;
}
inline float Sqrt(float s) {
	reg_store_ss(&s, reg_sqrt_ss(reg_load_ss(&s)));
	return s;
}
inline float RSqrt(float s) {
	return Rcp22Bit(spn::Sqrt(s));
}

constexpr static uint32_t Fullbit = 0xffffffff,
						Absbit = 0x7fffffff;
constexpr float FLOAT_EPSILON = 1e-5f;		//!< 2つの値を同一とみなす誤差
// ベクトルレジスタ用の定数
class xmm_const {
	protected:
		static void Initialize();
		static void Terminate();

	#define DefXmm(name) \
		private: static reg128 _##name; \
		public: static const reg128& name() { return _##name; }
	DefXmm(tmp0001)
	DefXmm(tmp0001_i)
	DefXmm(tmp0111)
	DefXmm(tmp0111_i)
	DefXmm(tmp0000)
	DefXmm(tmp0000_i)
	DefXmm(tmp1000)
	DefXmm(tmp1000_i)
	DefXmm(tmp1111)
	DefXmm(tmp1111_i)
	DefXmm(epsilon)
	DefXmm(epsilonM)
	DefXmm(minus0)
	DefXmm(fullbit)
	DefXmm(absmask)
	#undef DefXmm

	using XmmA4 = const reg128 (&)[4];
	#define DefXmmA(name, n) \
		private: static reg128 _##name[n]; \
		public: static XmmA4 name() { return _##name; }
	DefXmmA(mask, 4)
	DefXmmA(maskN, 4)
	DefXmmA(matI, 4)
	#undef DefXmmA

	using FA44 = const float (&)[4][4];
	private: static float _cs_matI[4][4];
	public: static FA44 cs_matI() { return _cs_matI; }
};
}

#include "structure/niftycounter.hpp"
namespace spn {
DEF_NIFTY_INITIALIZER(xmm_const)
//! レジスタ要素が全てゼロか判定 (+0 or -0)
inline bool _mmIsZero(reg128 r) {
	r = reg_andnot_ps(xmm_const::minus0(), r);
	r = reg_cmpeq_ps(r, xmm_const::tmp0000());
	return reg_movemask_ps(r) == 0;
}
//! 誤差を含んだゼロ判定
inline bool _mmIsZeroEps(reg128 r) {
	reg128 xm0 = reg_cmplt_ps(r, xmm_const::epsilon()),
			xm1 = reg_cmpnle_ps(r, xmm_const::epsilonM());
	xm0 = reg_or_ps(xm0, xm1);
	return reg_movemask_ps(xm0) == 0;
}

template <int A, int B, int C, int D>
inline reg128 _makeMask() {
	reg128 zero = reg_setzero_ps();
	reg128 full = reg_andnot_ps(zero, zero);
	const int tmp2 = (((~D)&1)<<3)|(((~C)&1)<<2)|(((~B)&1)<<1)|((~A)&1);
	reg128 tmp = reg_move_ss(zero, full);
	return reg_shuffle_ps(tmp, tmp, tmp2);
}
template <class T>
constexpr T Pi = T(3.1415926535);	// std::atan(1.0f)*4
constexpr float PI = Pi<float>,
				SIN0 = 0,
				SIN30 = 1.f/2,
				SIN45 = 1.f/1.41421356f,
				SIN60 = 1.7320508f/2,
				SIN90 = 1.f,
				COS0 =  SIN90,
				COS30 = SIN60,
				COS45 = SIN45,
				COS60 = SIN30,
				COS90 = SIN0;
template <class T>
T Square(const T& t0) { return t0*t0; }
template <class T>
T Cubic(const T& t0) { return t0*t0*t0; }
// 代わりにDegree::ToRadian(ang)を使う
inline float DEGtoRAD(float ang) {
	return ang * spn::Rcp22Bit(180.f) * PI;
}
// 代わりにRadian::ToDegree(ang)を使う
inline float RADtoDEG(float ang) {
	return ang * spn::Rcp22Bit(PI) * 180.f;
}

template <class T>
struct TrivialWrapperBase : boost::serialization::traits<T,
							boost::serialization::object_serializable,
							boost::serialization::track_selectively>
{};
//! Tをtrivialなctorでラップ
template <class T>
struct TrivialWrapper : TrivialWrapperBase<TrivialWrapper<T>> {
	uint8_t _buff[sizeof(T)];

	T& operator * () { return (T&)*this; }
	const T& operator * () const { return (const T&)*this; }
	operator T& () { return *reinterpret_cast<T*>(_buff); }
	operator const T& () const { return *reinterpret_cast<const T*>(_buff); }
	T* operator -> () { return reinterpret_cast<T*>(_buff); }
	const T* operator -> () const { return reinterpret_cast<const T*>(_buff); }
	TrivialWrapper<T>& operator = (const T& t) {
		((T&)(*this)) = t;
		return *this;
	}

	friend class boost::serialization::access;
	template <class Archive>
	void serialize(Archive& ar, const unsigned int /*ver*/) {
		ar & boost::serialization::make_nvp("value", static_cast<T&>(*this));
	}

	template <class TA>
	bool operator == (const TA& t) const {
		return static_cast<const T&>(*this) == static_cast<const T&>(t);
	}
	template <class TA>
	bool operator != (const TA& t) const {
		return static_cast<const T&>(*this) != static_cast<const T&>(t);
	}
};
template <class T, int N>
struct TrivialWrapper<T[N]> : TrivialWrapperBase<TrivialWrapper<T[N]>> {
	T _buff[N];
	using AR = T (&)[N];
	using AR_C = const T (&)[N];

	AR operator * () { return _buff; }
	AR_C operator * () const { return _buff; }
	operator AR () { return _buff; }
	T& operator [] (int n) { return _buff[n]; }
	const T& operator [] (int n) const { return _buff[n]; }

	friend class boost::serialization::access;
	template <class Archive>
	void serialize(Archive& ar, const unsigned int /*ver*/) {
		ar & boost::serialization::make_nvp("value", _buff);
	}

	template <class TA>
	bool operator == (const TA& t) const {
		for(int i=0 ; i<N ; i++) {
			if(_buff[i] != t[i])
				return false;
		}
		return true;
	}
	template <class TA>
	bool operator != (const TA& t) const {
		return !(this->operator == (t));
	}
};
}

// 次元毎のロード/ストア関数を定義
// LOADPS_[ZeroFlag][AlignedFlag][Dim]
#define LOADPS_A4(ptr)		reg_load_ps(ptr)			// アラインメント済み4次元ベクトル読み込み
#define LOADPS_4(ptr)		reg_loadu_ps(ptr)
#define LOADPS_ZA4(ptr)		LOADPS_A4(ptr)				// 空き要素がゼロ初期化付き
#define LOADPS_Z4(ptr)		LOADPS_4(ptr)
#define LOADPS_IA4(ptr,n)	LOADPS_A4(ptr)				// 空きかつ対角要素のみが1、他は0
#define LOADPS_I4(ptr,n)	LOADPS_4(ptr)
#define STOREPS_A4(ptr, r)	reg_store_ps(ptr, r)		// アラインメント済み4次元ベクトル書き込み
#define STOREPS_4(ptr, r)	reg_storeu_ps(ptr, r)

#define LOADPS_A3(ptr)		LOADPS_A4(ptr)
#define LOADPS_3(ptr)		LOADPS_4(ptr)
#define LOADPS_BASE3(ptr,src,lfunc)	reg_mul_ps(src, lfunc(ptr))
#define LOADPS_ZA3(ptr)		reg_and_ps(spn::xmm_const::tmp0111_i(), reg_load_ps(ptr))
#define LOADPS_Z3(ptr)		reg_and_ps(spn::xmm_const::tmp0111_i(), reg_loadu_ps(ptr))
#define LOADPS_IA3(ptr,n)	BOOST_PP_IF(BOOST_PP_EQUAL(n,3), \
										reg_or_ps(spn::xmm_const::matI()[3], LOADPS_ZA3(ptr)), \
										LOADPS_ZA3(ptr))
#define LOADPS_I3(ptr,n)	BOOST_PP_IF(BOOST_PP_EQUAL(n,3), \
										reg_or_ps(spn::xmm_const::matI()[3], LOADPS_Z3(ptr)), \
										LOADPS_Z3(ptr))
#define STOREPS_A3(ptr, r)	{ reg_storel_pi((reg64i*)ptr, r); reg_store_ss(ptr+2, reg_shuffle_ps(r, r, _REG_SHUFFLE(2,2,2,2))); }
#define STOREPS_3(ptr, r)	STOREPS_A3(ptr,r)

#define LOADPS_A2(ptr)		LOADPS_A4(ptr)
#define LOADPS_2(ptr)		LOADPS_4(ptr)
#define LOADPS_ZA2(ptr)		reg_loadl_pi(spn::xmm_const::tmp0000(), (const reg64i*)ptr)
#define LOADPS_Z2(ptr)		LOADPS_ZA2(ptr)
#define LOADPS_IA2(ptr,n)	reg_loadl_pi(spn::xmm_const::matI()[n], (const reg64i*)ptr)
#define LOADPS_I2(ptr,n)	LOADPS_IA2(ptr,n)
#define STOREPS_A2(ptr, r)	reg_storel_pi((reg64i*)ptr,r)
#define STOREPS_2(ptr, r)	STOREPS_A2(ptr,r)

#define BOOLNIZE(b) BOOST_PP_IF(b,true,false)
#define AFLAG(a) BOOST_PP_IF(a,A,NOTHING)

//! アライン済み行列の隙間を埋める変数定義
#define GAP_MATRIX(matname, m, n, seq) GAP_MATRIX_DEF(NOTHING , matname, m, n, seq)
//! constやmutableを付ける場合の定義
#define GAP_MATRIX_DEF(prefix, matname, m, n, seq) union { prefix BOOST_PP_CAT(BOOST_PP_CAT(spn::AMat,m),n) matname; struct { BOOST_PP_SEQ_FOR_EACH_I(GAP_TFUNC_OUTER, n, seq) }; };
#define GAP_DUMMY(aux,index,amount) BOOST_PP_CAT(BOOST_PP_CAT(BOOST_PP_CAT(float dummy, __LINE__), aux), index)[amount];
//! Tupleの要素最後尾のsizeofを足し合わせる
#define COUNT_SIZE(tup)			((BOOST_PP_REPEAT(BOOST_PP_TUPLE_SIZE(tup), COUNT_SIZE2, tup)+3)/4)
#define COUNT_SIZE2(z,idx,data)	+sizeof(GET_LAST(BOOST_PP_TUPLE_ELEM(idx,data)))
#define GET_LAST(tup) BOOST_PP_TUPLE_ELEM(BOOST_PP_DEC(BOOST_PP_TUPLE_SIZE(tup)), tup)
//! 要素のサイズを考慮しつつ, 16byte alignedになるよう前後に適切なパディングを入れる
#define GAP_TFUNC_OUTER(z,nAr,idx,elem) \
		GAP_DUMMY(_,idx,nAr) \
		BOOST_PP_REPEAT(BOOST_PP_TUPLE_SIZE(elem), GAP_TFUNC_INNER, elem) \
		GAP_DUMMY(_B,idx,4-nAr-COUNT_SIZE(elem))
//! 奇数要素と偶数要素を連結して変数宣言とする
#define GAP_TFUNC_INNER(z,idx,data) BOOST_PP_IF(BOOST_PP_EQUAL(BOOST_PP_TUPLE_SIZE(BOOST_PP_TUPLE_ELEM(idx,data)),2), GAP_TFUNC_INNER2, GAP_TFUNC_INNER3)(BOOST_PP_TUPLE_ELEM(idx,data))
#define GAP_TFUNC_INNER2(tup) ARGPAIR(BOOST_PP_TUPLE_ELEM(0,tup), BOOST_PP_TUPLE_ELEM(1,tup))
#define GAP_TFUNC_INNER3(tup) BOOST_PP_TUPLE_ELEM(0,tup) ARGPAIR(BOOST_PP_TUPLE_ELEM(1,tup), BOOST_PP_TUPLE_ELEM(2,tup))
#define ARGPAIR(a,b)	spn::TrivialWrapper<a> b;

//! アライン済みベクトルの隙間を埋める定義
#define GAP_VECTOR(vecname, n, seq) GAP_VECTOR_DEF(NOTHING, vecname, n, seq)
#define GAP_VECTOR_DEF(prefix, vecname, n, seq)	union { prefix BOOST_PP_CAT(spn::AVec, n) vecname; struct { float _dummy[n]; BOOST_PP_SEQ_FOR_EACH(GAP_TFUNC_VEC, NOTHING, seq) }; };
#define GAP_TFUNC_VEC(z,dummy,elem)	elem;

// ----------- ベクトルや行列のプロトタイプ定義 -----------
namespace spn {
	enum AXIS_FLAG {
		AXIS_X,
		AXIS_Y,
		AXIS_Z,
		NUM_AXIS
	};

	template <int N, bool A>
	struct VecT;

	struct MatBase {
		//! 対角線上に数値を設定。残りはゼロ
		static struct _TagDiagonal {} TagDiagonal;
		static struct _TagIdentity {} TagIdentity;
		//! 全てを対象
		static struct _TagAll {} TagAll;
	};
	template <int M, int N, bool A>
	struct MatT;

	template <bool A>
	struct PlaneT;

	struct QuatBase {
		static struct _TagIdentity {} TagIdentity;
	};
	template <bool A>
	struct QuatT;
	template <bool A>
	struct ExpQuatT;

	//! Vector型の時はtrue_type, それ以外はfalse_type
	template <class T>
	struct IsVectorT : std::false_type {};
	template <int N, bool A>
	struct IsVectorT<spn::VecT<N,A>> : std::true_type {};
	//! Matrix型の時はtrue_type, それ以外はfalse_type
	template <class T>
	struct IsMatrixT : std::false_type {};
	template <int M, int N, bool A>
	struct IsMatrixT<spn::MatT<M,N,A>> : std::true_type {};
	//! Quaternion型の時はtrue_type, それ以外はfalse_type
	template <class T>
	struct IsQuatT : std::false_type {};
	template <bool A>
	struct IsQuatT<spn::QuatT<A>> : std::true_type {};
}
namespace std {
	template <int N, bool A>
	struct hash<spn::VecT<N,A>> {
		size_t operator()(const spn::VecT<N,A>& v) const {
			auto* src = reinterpret_cast<const uint32_t*>(&v);
			size_t tmp = 0xc000feee;
			for(int i=0 ; i<N ; i++)
				tmp ^= size_t(src[i] << (i+3)) ^ src[i];
			return tmp;
		}
	};
	template <int M, int N, bool A>
	struct hash<spn::MatT<M,N,A>> {
		size_t operator()(const spn::MatT<M,N,A>& m) const {
			size_t tmp = 0xc000feee;
			for(int i=0 ; i<M ; i++) {
				auto* src = reinterpret_cast<const uint32_t*>(m.ma[i]);
				for(int j=0 ; j<N ; j++)
					tmp ^= (src[j] << (j+3)) ^ src[j];
			}
			return tmp;
		}
	};
	template <bool A>
	struct hash<spn::QuatT<A>> {
		size_t operator()(const spn::QuatT<A>& q) const {
			using VT = spn::VecT<4,A>;
			return hash<VT>()(reinterpret_cast<const VT&>(q));
		}
	};
}


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "gtest/gtest.h"

#include "bark/commons/params/setter_params.hpp"
#include "bark/geometry/polygon.hpp"
#include "bark/geometry/standard_shapes.hpp"
#include "bark/models/behavior/constant_acceleration/constant_acceleration.hpp"
#include "bark/models/behavior/rule_based/mobil.hpp"
#include "bark/models/behavior/rule_based/mobil_behavior.hpp"
#include "bark/models/dynamic/single_track.hpp"
#include "bark/models/execution/interpolation/interpolate.hpp"
#include "bark/world/goal_definition/goal_definition_polygon.hpp"
#include "bark/world/objects/agent.hpp"
#include "bark/world/observed_world.hpp"
#include "bark/world/tests/make_test_xodr_map.hpp"

using namespace bark::models::dynamic;
using namespace bark::models::execution;
using namespace bark::commons;
using namespace bark::models::behavior;
using namespace bark::world::map;
using namespace bark::models::dynamic;

using bark::geometry::Model3D;
using bark::geometry::Point2d;
using bark::geometry::Polygon;
using bark::geometry::Pose;
using bark::geometry::standard_shapes::CarRectangle;
using bark::geometry::standard_shapes::GenerateGoalRectangle;
using bark::world::ObservedWorld;
using bark::world::World;
using bark::world::WorldPtr;
using bark::world::goal_definition::GoalDefinitionPolygon;
using bark::world::objects::Agent;
using bark::world::objects::AgentPtr;
using bark::world::tests::MakeXodrMapEndingLaneInParallel;
using bark::world::tests::MakeXodrMapOneRoadTwoLanes;

ObservedWorld make_observed_world_mobil(double vel, ParamsPtr params) {
  // Setting Up Map
  auto open_drive_map = MakeXodrMapOneRoadTwoLanes();
  auto map_interface = std::make_shared<MapInterface>();
  map_interface->interface_from_opendrive(open_drive_map);

  Polygon car_polygon = CarRectangle();

  Polygon polygon = GenerateGoalRectangle(6, 3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(100, -2))));  // < move the goal polygon into the driving
                                // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  ExecutionModelPtr exec_model(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model(new SingleTrackModel(params));
  BehaviorModelPtr beh_model(new BehaviorMobilRuleBased(params));

  State init_state1(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state1 << 0.0, 53.0, -1.75, 0.0, vel;
  AgentPtr agent1(new Agent(init_state1, beh_model, dyn_model, exec_model,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Preceding Agent
  ExecutionModelPtr exec_model2(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model2(new SingleTrackModel(params));
  BehaviorModelPtr beh_model2(new BehaviorConstantAcceleration(params));

  State init_state2(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state2 << 0.0, 80.0, -1.75, 0.0, vel;
  AgentPtr agent2(new Agent(init_state2, beh_model2, dyn_model2, exec_model2,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Agent coming from behind on the right
  ExecutionModelPtr exec_model3(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model3(new SingleTrackModel(params));
  BehaviorModelPtr beh_model3(new BehaviorConstantAcceleration(params));

  State init_state3(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state3 << 0.0, 43.0, -1.75 - 3.5, 0.0, vel;
  AgentPtr agent3(new Agent(init_state3, beh_model3, dyn_model3, exec_model3,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Following Agent
  ExecutionModelPtr exec_model4(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model4(new SingleTrackModel(params));
  BehaviorModelPtr beh_model4(new BehaviorConstantAcceleration(params));

  State init_state4(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state4 << 0.0, 20.0, -1.75, 0.0, vel;
  AgentPtr agent4(new Agent(init_state4, beh_model4, dyn_model4, exec_model4,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Agent on the right in front
  ExecutionModelPtr exec_model5(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model5(new SingleTrackModel(params));
  BehaviorModelPtr beh_model5(new BehaviorConstantAcceleration(params));

  State init_state5(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state5 << 0.0, 100.0, -1.75 - 3.5, 0.0, vel;
  AgentPtr agent5(new Agent(init_state5, beh_model5, dyn_model5, exec_model5,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Construct World
  WorldPtr world(new World(params));
  world->AddAgent(agent1);
  world->AddAgent(agent2);
  world->AddAgent(agent3);
  world->AddAgent(agent4);
  world->AddAgent(agent5);
  world->UpdateAgentRTree();

  WorldPtr current_world_state(world->Clone());
  ObservedWorld observed_world(current_world_state, agent1->GetAgentId());

  return observed_world;
}

TEST(safety_not_met, behavior_mobil) {
  double vel = 5.0;
  auto params = std::make_shared<SetterParams>();
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", vel);
  params->SetReal("BehaviorMobilRuleBased::AThr", 0.2);
  params->SetReal("BehaviorMobilRuleBased::Politeness", 0.0);
  params->SetReal("BehaviorMobilRuleBased::BSafe", 1.0);

  ObservedWorld observed_world = make_observed_world_mobil(vel, params);

  const BehaviorModelPtr behavior_model =
      observed_world.GetEgoAgent()->GetBehaviorModel();
  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobilRuleBased>(behavior_model);
  behavior_mobil->SetLaneCorridor(observed_world.GetLaneCorridor());

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::KeepLane);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(impolite_incentive_met_safety_met, behavior_mobil) {
  double vel_ego = 5.0;
  auto params = std::make_shared<SetterParams>();
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", vel_ego);
  params->SetReal("BehaviorMobilRuleBased::AThr", 0.1);
  params->SetReal("BehaviorMobilRuleBased::Politeness", 0.0);
  params->SetReal("BehaviorMobilRuleBased::BSafe", 4.0);

  ObservedWorld observed_world = make_observed_world_mobil(vel_ego, params);

  const BehaviorModelPtr behavior_model =
      observed_world.GetEgoAgent()->GetBehaviorModel();
  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobilRuleBased>(behavior_model);
  behavior_mobil->SetLaneCorridor(observed_world.GetLaneCorridor());

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::ChangeLane);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(polite_incentive_not_met_safety_met, behavior_mobil) {
  double vel_ego = 5.0;
  auto params = std::make_shared<SetterParams>();
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", vel_ego);
  params->SetReal("BehaviorMobilRuleBased::AThr", 0.2);
  params->SetReal("BehaviorMobilRuleBased::Politeness", 1.0);
  params->SetReal("BehaviorMobilRuleBased::BSafe", 4.0);

  ObservedWorld observed_world = make_observed_world_mobil(vel_ego, params);

  const BehaviorModelPtr behavior_model =
      observed_world.GetEgoAgent()->GetBehaviorModel();
  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobilRuleBased>(behavior_model);
  behavior_mobil->SetLaneCorridor(observed_world.GetLaneCorridor());

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::KeepLane);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(polite_incentive_met_safety_met, behavior_mobil) {
  double vel_ego = 5.0;
  auto params = std::make_shared<SetterParams>();
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", vel_ego);
  params->SetReal("BehaviorMobilRuleBased::AThr", -5.0);  // HACK
  params->SetReal("BehaviorMobilRuleBased::Politeness", 1.0);
  params->SetReal("BehaviorMobilRuleBased::BSafe", 4.0);

  ObservedWorld observed_world = make_observed_world_mobil(vel_ego, params);

  const BehaviorModelPtr behavior_model =
      observed_world.GetEgoAgent()->GetBehaviorModel();
  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobilRuleBased>(behavior_model);
  behavior_mobil->SetLaneCorridor(observed_world.GetLaneCorridor());

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::ChangeLane);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(change_lane_due_to_lane_ending, behavior_mobil) {
  double vel_ego = 5.0;
  auto params = std::make_shared<SetterParams>();
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", vel_ego);
  params->SetBool("BehaviorIDMClassic::BrakeForLaneEnd", true);
  params->SetReal("BehaviorIDMClassic::BrakeForLaneEndEnabledDistance", 60);
  params->SetReal("BehaviorIDMClassic::MaxAcceleration", 1.9);
  params->SetReal("BehaviorMobilRuleBased::AThr", 0.2);
  params->SetReal("BehaviorMobilRuleBased::Politeness", 0.5);
  params->SetReal("BehaviorMobilRuleBased::BSafe", 4.0);

  // Setting Up Map
  auto open_drive_map = MakeXodrMapEndingLaneInParallel();
  auto map_interface = std::make_shared<MapInterface>();
  map_interface->interface_from_opendrive(open_drive_map);

  Polygon car_polygon = CarRectangle();

  Polygon polygon = GenerateGoalRectangle(6, 3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(100, -2))));  // < move the goal polygon into the driving
                                // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  ExecutionModelPtr exec_model(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model(new SingleTrackModel(params));
  BehaviorModelPtr beh_model(new BehaviorMobilRuleBased(params));

  State init_state1(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state1 << 0.0, 30.0, -1.75 - 3.5, 0.0, vel_ego;
  AgentPtr agent1(new Agent(init_state1, beh_model, dyn_model, exec_model,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Construct World
  WorldPtr world(new World(params));
  world->AddAgent(agent1);
  world->UpdateAgentRTree();

  WorldPtr current_world_state(world->Clone());
  ObservedWorld observed_world(current_world_state, agent1->GetAgentId());

  const BehaviorModelPtr behavior_model = agent1->GetBehaviorModel();

  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobilRuleBased>(behavior_model);
  behavior_mobil->SetLaneCorridor(observed_world.GetLaneCorridor());

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::ChangeLane);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(no_lane_change_to_ending_lane, behavior_mobil) {
  double vel = 5.0;
  auto params = std::make_shared<SetterParams>();
  params->SetReal("BehaviorIDMClassic::DesiredVelocity", vel);
  params->SetBool("BehaviorIDMClassic::BrakeForLaneEnd", true);
  params->SetReal("BehaviorIDMClassic::BrakeForLaneEndEnabledDistance", 60);
  params->SetReal("BehaviorIDMClassic::MaxAcceleration", 1.9);
  params->SetReal("BehaviorMobilRuleBased::AThr", 0.1);
  params->SetReal("BehaviorMobilRuleBased::Politeness", 0.5);
  params->SetReal("BehaviorMobilRuleBased::BSafe", 4.0);

  // Setting Up Map
  auto open_drive_map = MakeXodrMapEndingLaneInParallel();
  auto map_interface = std::make_shared<MapInterface>();
  map_interface->interface_from_opendrive(open_drive_map);

  Polygon car_polygon = CarRectangle();

  Polygon polygon = GenerateGoalRectangle(6, 3);
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(100, -2))));  // < move the goal polygon into the driving
                                // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  ExecutionModelPtr exec_model(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model(new SingleTrackModel(params));
  BehaviorModelPtr beh_model(new BehaviorMobilRuleBased(params));

  State init_state1(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state1 << 0.0, 30.0, -1.75, 0.0, vel;
  AgentPtr agent1(new Agent(init_state1, beh_model, dyn_model, exec_model,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Preceding Agent
  ExecutionModelPtr exec_model2(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model2(new SingleTrackModel(params));
  BehaviorModelPtr beh_model2(new BehaviorConstantAcceleration(params));

  State init_state2(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state2 << 0.0, 60.0, -1.75, 0.0, vel;
  AgentPtr agent2(new Agent(init_state2, beh_model2, dyn_model2, exec_model2,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Construct World
  WorldPtr world(new World(params));
  world->AddAgent(agent1);
  world->AddAgent(agent2);
  world->UpdateAgentRTree();

  WorldPtr current_world_state(world->Clone());
  ObservedWorld observed_world(current_world_state, agent1->GetAgentId());

  const BehaviorModelPtr behavior_model = agent1->GetBehaviorModel();

  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobilRuleBased>(behavior_model);
  behavior_mobil->SetLaneCorridor(observed_world.GetLaneCorridor());

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::KeepLane);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(slower_preceding_agent, behavior_mobil) {
  // Setting Up Map
  auto open_drive_map = MakeXodrMapOneRoadTwoLanes();
  auto map_interface = std::make_shared<MapInterface>();
  map_interface->interface_from_opendrive(open_drive_map);

  auto params = std::make_shared<SetterParams>();
  Polygon car_polygon = CarRectangle();

  Polygon polygon(
      Pose(1, 1, 0),
      std::vector<Point2d>{Point2d(0, 0), Point2d(0, 2), Point2d(2, 2),
                           Point2d(2, 0), Point2d(0, 0)});
  std::shared_ptr<Polygon> goal_polygon(
      std::dynamic_pointer_cast<Polygon>(polygon.Translate(
          Point2d(50, -2))));  // < move the goal polygon into the driving
                               // corridor in front of the ego vehicle
  auto goal_definition_ptr =
      std::make_shared<GoalDefinitionPolygon>(*goal_polygon);

  // Ego Agent
  ExecutionModelPtr exec_model(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model(new SingleTrackModel(params));
  BehaviorModelPtr beh_model(new BehaviorMobil(params));

  State init_state1(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state1 << 0.0, 3.0, -1.75, 0.0, 5.0;
  AgentPtr agent1(new Agent(init_state1, beh_model, dyn_model, exec_model,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Preceding Agent
  ExecutionModelPtr exec_model2(new ExecutionModelInterpolate(params));
  DynamicModelPtr dyn_model2(new SingleTrackModel(params));
  BehaviorModelPtr beh_model2(new BehaviorMobil(params));

  State init_state2(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state2 << 0.0, 15.0, -1.75, 0.0, 2.0;
  AgentPtr agent2(new Agent(init_state2, beh_model2, dyn_model2, exec_model2,
                            car_polygon, params, goal_definition_ptr,
                            map_interface, bark::geometry::Model3D()));

  // Construct World
  WorldPtr world(new World(params));
  world->AddAgent(agent1);
  world->AddAgent(agent2);
  world->UpdateAgentRTree();

  WorldPtr current_world_state(world->Clone());
  ObservedWorld observed_world(current_world_state, agent1->GetAgentId());

  const BehaviorModelPtr behavior_model = agent1->GetBehaviorModel();

  auto behavior_mobil =
      std::dynamic_pointer_cast<BehaviorMobil>(behavior_model);

  LaneChangeDecision decision;
  LaneCorridorPtr lane_corr;
  std::tie(decision, lane_corr) =
      behavior_mobil->CheckIfLaneChangeBeneficial(observed_world);
  EXPECT_EQ(decision, LaneChangeDecision::ChangeRight);
  BARK_EXPECT_TRUE(lane_corr != nullptr);
}

TEST(behavior_mobil, clone) {
  auto params = std::make_shared<SetterParams>();
  BehaviorModelPtr beh_model(new BehaviorMobil(params));
  ASSERT_EQ(BehaviorStatus::VALID, beh_model->GetBehaviorStatus());
  beh_model->SetBehaviorStatus(BehaviorStatus::EXPIRED);

  auto cloned_model = BehaviorModelPtr(beh_model->Clone());
  ASSERT_EQ(BehaviorStatus::EXPIRED, cloned_model->GetBehaviorStatus());
}

int main(int argc, char** argv) {
  // FLAGS_v=2;
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <utility>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/FixedHashMap.hpp"
#include "DataStructures/Index.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/DirectionMap.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "Evolution/Systems/NewtonianEuler/ConservativeFromPrimitive.hpp"
#include "Evolution/Systems/NewtonianEuler/Tags.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"

namespace NewtonianEuler::fd {
template <typename PrimsTags, typename TagsList, size_t Dim,
          size_t ThermodynamicDim, typename F>
void reconstruct_prims_work(
    const gsl::not_null<std::array<Variables<TagsList>, Dim>*>
        vars_on_lower_face,
    const gsl::not_null<std::array<Variables<TagsList>, Dim>*>
        vars_on_upper_face,
    const F& reconstruct, const Variables<PrimsTags>& volume_prims,
    const EquationsOfState::EquationOfState<false, ThermodynamicDim>& eos,
    const Element<Dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(Dim),
        std::pair<Direction<Dim>, ElementId<Dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>& neighbor_data,
    const Mesh<Dim>& subcell_mesh, const size_t ghost_zone_size) {
  // Conservative vars tags
  using MassDensityCons = Tags::MassDensityCons;
  using EnergyDensity = Tags::EnergyDensity;
  using MomentumDensity = Tags::MomentumDensity<Dim>;

  // Primitive vars tags
  using MassDensity = Tags::MassDensity<DataVector>;
  using Velocity = Tags::Velocity<DataVector, Dim>;
  using SpecificInternalEnergy = Tags::SpecificInternalEnergy<DataVector>;
  using Pressure = Tags::Pressure<DataVector>;

  using prim_tags_for_reconstruction =
      tmpl::list<MassDensity, Velocity, Pressure>;

  ASSERT(Mesh<Dim>(subcell_mesh.extents(0), subcell_mesh.basis(0),
                   subcell_mesh.quadrature(0)) == subcell_mesh,
         "The subcell mesh should be isotropic but got " << subcell_mesh);
  const size_t volume_num_pts = subcell_mesh.number_of_grid_points();
  const size_t reconstructed_num_pts =
      (subcell_mesh.extents(0) + 1) *
      subcell_mesh.extents().slice_away(0).product();
  const size_t neighbor_num_pts =
      ghost_zone_size * subcell_mesh.extents().slice_away(0).product();
  size_t vars_in_neighbor_count = 0;
  tmpl::for_each<prim_tags_for_reconstruction>(
      [&element, &neighbor_data, neighbor_num_pts, &reconstruct,
       reconstructed_num_pts, volume_num_pts, &volume_prims,
       &vars_in_neighbor_count, &vars_on_lower_face, &vars_on_upper_face,
       &subcell_mesh](auto tag_v) {
        using tag = tmpl::type_from<decltype(tag_v)>;
        auto& volume_tensor = get<tag>(volume_prims);

        const size_t number_of_components = volume_tensor.size();
        const gsl::span<const double> volume_vars = gsl::make_span(
            volume_tensor[0].data(), number_of_components * volume_num_pts);
        std::array<gsl::span<double>, Dim> upper_face_vars{};
        std::array<gsl::span<double>, Dim> lower_face_vars{};
        for (size_t i = 0; i < Dim; ++i) {
          gsl::at(upper_face_vars, i) = gsl::make_span(
              get<tag>(gsl::at(*vars_on_upper_face, i))[0].data(),
              number_of_components * reconstructed_num_pts);
          gsl::at(lower_face_vars, i) = gsl::make_span(
              get<tag>(gsl::at(*vars_on_lower_face, i))[0].data(),
              number_of_components * reconstructed_num_pts);
        }

        DirectionMap<Dim, gsl::span<const double>> ghost_cell_vars{};
        for (const auto& direction : Direction<Dim>::all_directions()) {
          const auto& neighbors_in_direction =
              element.neighbors().at(direction);
          ASSERT(neighbors_in_direction.size() == 1,
                 "Currently only support one neighbor in each direction, but "
                 "got "
                     << neighbors_in_direction.size() << " in direction "
                     << direction);
          ghost_cell_vars[direction] = gsl::make_span(
              &neighbor_data.at(std::pair{
                  direction,
                  *neighbors_in_direction
                       .begin()})[vars_in_neighbor_count * neighbor_num_pts],
              number_of_components * neighbor_num_pts);
        }

        reconstruct(make_not_null(&upper_face_vars),
                    make_not_null(&lower_face_vars), volume_vars,
                    ghost_cell_vars, subcell_mesh.extents(),
                    number_of_components);

        vars_in_neighbor_count += number_of_components;
      });

  for (size_t i = 0; i < Dim; ++i) {
    auto& vars_upper_face = gsl::at(*vars_on_upper_face, i);
    auto& vars_lower_face = gsl::at(*vars_on_lower_face, i);

    if constexpr (ThermodynamicDim == 2) {
      get<SpecificInternalEnergy>(vars_upper_face) =
          eos.specific_internal_energy_from_density_and_pressure(
              get<MassDensity>(vars_upper_face),
              get<Pressure>(vars_upper_face));
      get<SpecificInternalEnergy>(vars_lower_face) =
          eos.specific_internal_energy_from_density_and_pressure(
              get<MassDensity>(vars_lower_face),
              get<Pressure>(vars_lower_face));
    } else {
      get<SpecificInternalEnergy>(vars_upper_face) =
          eos.specific_internal_energy_from_density(
              get<MassDensity>(vars_upper_face));
      get<SpecificInternalEnergy>(vars_lower_face) =
          eos.specific_internal_energy_from_density(
              get<MassDensity>(vars_lower_face));
    }

    // Compute conserved variables on faces
    NewtonianEuler::ConservativeFromPrimitive<Dim>::apply(
        make_not_null(&get<MassDensityCons>(vars_upper_face)),
        make_not_null(&get<MomentumDensity>(vars_upper_face)),
        make_not_null(&get<EnergyDensity>(vars_upper_face)),
        get<MassDensity>(vars_upper_face), get<Velocity>(vars_upper_face),
        get<SpecificInternalEnergy>(vars_upper_face));
    NewtonianEuler::ConservativeFromPrimitive<Dim>::apply(
        make_not_null(&get<MassDensityCons>(vars_lower_face)),
        make_not_null(&get<MomentumDensity>(vars_lower_face)),
        make_not_null(&get<EnergyDensity>(vars_lower_face)),
        get<MassDensity>(vars_lower_face), get<Velocity>(vars_lower_face),
        get<SpecificInternalEnergy>(vars_lower_face));
  }
}

template <typename TagsList, typename PrimsTags, size_t Dim,
          size_t ThermodynamicDim, typename F0, typename F1>
void reconstruct_fd_neighbor_work(
    const gsl::not_null<Variables<TagsList>*> vars_on_face,
    const F0& reconstruct_lower_neighbor, const F1& reconstruct_upper_neighbor,
    const Variables<PrimsTags>& subcell_volume_prims,
    const EquationsOfState::EquationOfState<false, ThermodynamicDim>& eos,
    const Element<Dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(Dim),
        std::pair<Direction<Dim>, ElementId<Dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>& neighbor_data,
    const Mesh<Dim>& subcell_mesh,
    const Direction<Dim>& direction_to_reconstruct,
    const size_t ghost_zone_size) {
  // Conservative vars tags
  using MassDensityCons = Tags::MassDensityCons;
  using EnergyDensity = Tags::EnergyDensity;
  using MomentumDensity = Tags::MomentumDensity<Dim>;

  // Primitive vars tags
  using MassDensity = Tags::MassDensity<DataVector>;
  using Velocity = Tags::Velocity<DataVector, Dim>;
  using SpecificInternalEnergy = Tags::SpecificInternalEnergy<DataVector>;
  using Pressure = Tags::Pressure<DataVector>;

  using prim_tags_for_reconstruction =
      tmpl::list<MassDensity, Velocity, Pressure>;

  const std::pair mortar_id{
      direction_to_reconstruct,
      *element.neighbors().at(direction_to_reconstruct).begin()};
  Index<Dim> ghost_data_extents = subcell_mesh.extents();
  ghost_data_extents[direction_to_reconstruct.dimension()] = ghost_zone_size;
  Variables<prim_tags_for_reconstruction> neighbor_prims{
      ghost_data_extents.product()};
  {
    ASSERT(neighbor_data.contains(mortar_id),
           "The neighbor data does not contain the mortar: ("
               << mortar_id.first << ',' << mortar_id.second << ")");
    const auto& neighbor_data_in_direction = neighbor_data.at(mortar_id);
    std::copy(neighbor_data_in_direction.begin(),
              std::next(neighbor_data_in_direction.begin(),
                        static_cast<std::ptrdiff_t>(
                            neighbor_prims.number_of_independent_components *
                            ghost_data_extents.product())),
              neighbor_prims.data());
  }

  tmpl::for_each<prim_tags_for_reconstruction>(
      [&direction_to_reconstruct, &ghost_data_extents, &neighbor_prims,
       &reconstruct_lower_neighbor, &reconstruct_upper_neighbor, &subcell_mesh,
       &subcell_volume_prims, &vars_on_face](auto tag_v) {
        using tag = tmpl::type_from<decltype(tag_v)>;
        const auto& tensor_volume = get<tag>(subcell_volume_prims);
        const auto& tensor_neighbor = get<tag>(neighbor_prims);
        auto& tensor_on_face = get<tag>(*vars_on_face);
        if (direction_to_reconstruct.side() == Side::Upper) {
          for (size_t tensor_index = 0; tensor_index < tensor_on_face.size();
               ++tensor_index) {
            reconstruct_upper_neighbor(
                make_not_null(&tensor_on_face[tensor_index]),
                tensor_volume[tensor_index], tensor_neighbor[tensor_index],
                subcell_mesh.extents(), ghost_data_extents,
                direction_to_reconstruct);
          }
        } else {
          for (size_t tensor_index = 0; tensor_index < tensor_on_face.size();
               ++tensor_index) {
            reconstruct_lower_neighbor(
                make_not_null(&tensor_on_face[tensor_index]),
                tensor_volume[tensor_index], tensor_neighbor[tensor_index],
                subcell_mesh.extents(), ghost_data_extents,
                direction_to_reconstruct);
          }
        }
      });

  if constexpr (ThermodynamicDim == 2) {
    get<SpecificInternalEnergy>(*vars_on_face) =
        eos.specific_internal_energy_from_density_and_pressure(
            get<MassDensity>(*vars_on_face), get<Pressure>(*vars_on_face));
  } else {
    get<SpecificInternalEnergy>(*vars_on_face) =
        eos.specific_internal_energy_from_density(
            get<MassDensity>(*vars_on_face));
  }
  NewtonianEuler::ConservativeFromPrimitive<Dim>::apply(
      make_not_null(&get<MassDensityCons>(*vars_on_face)),
      make_not_null(&get<MomentumDensity>(*vars_on_face)),
      make_not_null(&get<EnergyDensity>(*vars_on_face)),
      get<MassDensity>(*vars_on_face), get<Velocity>(*vars_on_face),
      get<SpecificInternalEnergy>(*vars_on_face));
}
}  // namespace NewtonianEuler::fd


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/fast_gradients.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(cctbx::xray::fast_gradients<>)

namespace cctbx { namespace xray { namespace boost_python {

namespace {

  struct fast_gradients_wrappers
  {
    typedef fast_gradients<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t, bases<w_t::base_t> >("fast_gradients", no_init)
        .def(init<uctbx::unit_cell const&,
                  af::const_ref<scatterer<> > const&,
                  scattering_type_registry const&,
                  optional<double const&,
                           double const&,
                           double const&,
                           double const&> >(
          (arg("unit_cell"),
           arg("scatterers"),
           arg("scattering_type_registry"),
           arg("u_base"),
           arg("wing_cutoff"),
           arg("exp_table_one_over_step_size"),
           arg("tolerance_positive_definite"))))
        .def("sampling",
          (void(w_t::*)(
            af::const_ref<scatterer<> > const&,
            af::const_ref<double> const&,
            scattering_type_registry const&,
            sgtbx::site_symmetry_table const&,
            af::const_ref<double, w_t::accessor_type> const&,
            std::size_t,
            bool)) &w_t::sampling,
          (arg("scatterers"),
           arg("u_iso_refinable_params"),
           arg("scattering_type_registry"),
           arg("site_symmetry_table"),
           arg("ft_d_target_d_f_calc"),
           arg("n_parameters"),
           arg("sampled_density_must_be_positive")))
        .def("sampling",
          (void(w_t::*)(
            af::const_ref<scatterer<> > const&,
            af::const_ref<double> const&,
            scattering_type_registry const&,
            sgtbx::site_symmetry_table const&,
            af::const_ref<std::complex<double>, w_t::accessor_type> const&,
            std::size_t,
            bool)) &w_t::sampling,
          (arg("scatterers"),
           arg("u_iso_refinable_params"),
           arg("scattering_type_registry"),
           arg("site_symmetry_table"),
           arg("ft_d_target_d_f_calc"),
           arg("n_parameters"),
           arg("sampled_density_must_be_positive")))
        .def("packed", &w_t::packed)
        .def("d_target_d_site_cart", &w_t::d_target_d_site_cart)
        .def("d_target_d_u_iso", &w_t::d_target_d_u_iso)
        .def("d_target_d_u_cart", &w_t::d_target_d_u_cart)
        .def("d_target_d_occupancy", &w_t::d_target_d_occupancy)
        .def("d_target_d_fp", &w_t::d_target_d_fp)
        .def("d_target_d_fdp", &w_t::d_target_d_fdp)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_fast_gradients()
  {
    fast_gradients_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python


#include <chrono>
#include <iostream>
#include <iomanip>
#include <filesystem>
#include <fstream>
#include <boost/program_options.hpp>
#include "io_service_pool.hpp"
#include "safe_counter.hpp"
#include "random_forest.hpp"
#include "receptor.hpp"
#include "ligand.hpp"
#include "array.hpp"

int main(int argc, char* argv[])
{
	using namespace std;
	using namespace std::filesystem;
	path receptor_path, ligand_path, out_path;
	array<double, 3> center, size;
	size_t seed, num_threads, num_trees, num_tasks, max_conformations;
	double granularity;
	bool score_only = true;
	bool anna_test = true;

	// Process program options.
	try
	{
		// Initialize the default values of optional arguments.
		using namespace std::chrono;
		const path default_out_path = ".";
		const size_t default_seed = duration_cast<seconds>(system_clock::now().time_since_epoch()).count();
		const size_t default_num_threads = std::thread::hardware_concurrency();
		const size_t default_num_trees = 500;
		const size_t default_num_tasks = 64;
		const size_t default_max_conformations = 9;
		const double default_granularity = 0.125;

		// Set up options description.
		using namespace boost::program_options;
		options_description input_options("input (required)");
		input_options.add_options()
			("receptor", value<path>(&receptor_path)->required(), "receptor in PDBQT format")
			("ligand", value<path>(&ligand_path)->required(), "ligand or folder of ligands in PDBQT format")
			("center_x", value<double>(&center[0])->required(), "x coordinate of the search space center")
			("center_y", value<double>(&center[1])->required(), "y coordinate of the search space center")
			("center_z", value<double>(&center[2])->required(), "z coordinate of the search space center")
			("size_x", value<double>(&size[0])->required(), "size in the x dimension in Angstrom")
			("size_y", value<double>(&size[1])->required(), "size in the y dimension in Angstrom")
			("size_z", value<double>(&size[2])->required(), "size in the z dimension in Angstrom")
			;
		options_description output_options("output (optional)");
		output_options.add_options()
			("out", value<path>(&out_path)->default_value(default_out_path), "folder of predicted conformations in PDBQT format")
			;
		options_description miscellaneous_options("options (optional)");
		miscellaneous_options.add_options()
			("seed", value<size_t>(&seed)->default_value(default_seed), "explicit non-negative random seed")
			("threads", value<size_t>(&num_threads)->default_value(default_num_threads), "number of worker threads to use")
			("trees", value<size_t>(&num_trees)->default_value(default_num_trees), "number of decision trees in random forest")
			("tasks", value<size_t>(&num_tasks)->default_value(default_num_tasks), "number of Monte Carlo tasks for global search")
			("conformations", value<size_t>(&max_conformations)->default_value(default_max_conformations), "maximum number of binding conformations to write")
			("granularity", value<double>(&granularity)->default_value(default_granularity), "density of probe atoms of grid maps")
			("score_only", bool_switch(&score_only), "scoring without docking")
			("help", "this help information")
			("version", "version information")
			("config", value<path>(), "configuration file to load options from")
			;
		options_description all_options;
		all_options.add(input_options).add(output_options).add(miscellaneous_options);

		// Parse command line arguments.
		variables_map vm;
		store(parse_command_line(argc, argv, all_options), vm);

		// If no command line argument is supplied or help is requested, print the usage and exit.
		if (argc == 1 || vm.count("help"))
		{
			cout << all_options;
			return 0;
		}

		// If version is requested, print the version and exit.
		if (vm.count("version"))
		{
			cout << "2.2.3" << endl;
			return 0;
		}

		// If a configuration file is present, parse it.
		if (vm.count("config"))
		{
			ifstream config_file(vm["config"].as<path>());
			store(parse_config_file(config_file, all_options), vm);
		}

		// Notify the user of parsing errors, if any.
		vm.notify();

		// Validate receptor_path.
		if (!exists(receptor_path))
		{
			cerr << "Option receptor " << receptor_path << " does not exist" << endl;
			return 1;
		}
		if (!is_regular_file(receptor_path))
		{
			cerr << "Option receptor " << receptor_path << " is not a regular file" << endl;
			return 1;
		}

		// Validate ligand_path.
		if (!exists(ligand_path))
		{
			cerr << "Option ligand " << ligand_path << " does not exist" << endl;
			return 1;
		}

		// Validate out_path.
		if (exists(out_path))
		{
			if (!is_directory(out_path))
			{
				cerr << "Option out " << out_path << " is not a directory" << endl;
				return 1;
			}
		}
		else
		{
			if (!create_directories(out_path))
			{
				cerr << "Failed to create output folder " << out_path << endl;
				return 1;
			}
		}

		// Validate miscellaneous options.
		if (!num_threads)
		{
			cerr << "Option threads must be 1 or greater" << endl;
			return 1;
		}
		if (!num_tasks)
		{
			cerr << "Option tasks must be 1 or greater" << endl;
			return 1;
		}
		if (!max_conformations)
		{
			cerr << "Option conformations must be 1 or greater" << endl;
			return 1;
		}
		if (granularity <= 0)
		{
			cerr << "Option granularity must be positive" << endl;
			return 1;
		}
	}
	catch (const exception& e)
	{
		cerr << e.what() << endl;
		return 1;
	}

	cout << "Hello" << endl;
	// Parse the receptor.
	cout << "Parsing the receptor " << receptor_path << endl;
	receptor rec(receptor_path, center, size, granularity);

	// Reserve storage for result containers.
	vector<vector<result>> result_containers(num_tasks);
	for (auto& rc : result_containers)
	{
		rc.reserve(20);	// Maximum number of results obtained from a single Monte Carlo task.
	}
	vector<result> results;
	results.reserve(max_conformations);

	// Enumerate and sort input ligands.
	cout << "Enumerating input ligands in " << ligand_path << endl;
	vector<path> input_ligand_paths;
	if (is_regular_file(ligand_path))
	{
		input_ligand_paths.push_back(ligand_path);
	}
	else
	{
		for (directory_iterator dir_iter(ligand_path), end_dir_iter; dir_iter != end_dir_iter; ++dir_iter)
		{
			// Filter files with .pdbqt and .PDBQT extensions.
			const path input_ligand_path = dir_iter->path();
			const auto ext = input_ligand_path.extension();
			if (ext != ".pdbqt" && ext != ".PDBQT") continue;
			input_ligand_paths.push_back(input_ligand_path);
		}
	}
	const size_t num_input_ligands = input_ligand_paths.size();
	cout << "Sorting " << num_input_ligands << " input ligands in alphabetical order" << endl;
	sort(input_ligand_paths.begin(), input_ligand_paths.end());

	// Initialize a Mersenne Twister random number generator.
	cout << "Seeding a random number generator with " << seed << endl;
	mt19937_64 rng(seed);

	// Initialize an io service pool and create worker threads for later use.
	cout << "Creating an io service pool of " << num_threads << " worker threads" << endl;
	io_service_pool io(num_threads);
	safe_counter<size_t> cnt;

	// Precalculate the scoring function in parallel.
	cout << "Calculating a scoring function of " << scoring_function::n << " atom types" << endl;
	scoring_function sf;
	cnt.init((sf.n + 1) * sf.n >> 1);
	for (size_t t1 = 0; t1 < sf.n; ++t1)
	for (size_t t0 = 0; t0 <=  t1; ++t0)
	{
		io.post([&, t0, t1]()
		{
			sf.precalculate(t0, t1);
			cnt.increment();
		});
	}
	cnt.wait();
	sf.clear();

	// Train RF-Score on the fly.
	cout << "Training a random forest of " << num_trees << " trees with " << tree::nv << " variables and " << tree::ns << " samples" << endl;
	forest f(num_trees, seed);
	cnt.init(num_trees);
	for (size_t i = 0; i < num_trees; ++i)
	{
		io.post([&, i]()
		{
			f[i].train(8, f.u01_s);
			cnt.increment();
		});
	}
	cnt.wait();
	f.clear();

	// Output headers to the standard output and the log file.
	cout << "Creating GRID maps of " << granularity << " A and running " << num_tasks << " Monte Carlo searches per ligand" << endl
		<< "   Index             Ligand   nConfs   idock score (kcal/mol)   RF-Score (pKd)" << endl << setprecision(2);
	cout.setf(ios::fixed, ios::floatfield);
	ofstream log(out_path / "log.csv");
	log.setf(ios::fixed, ios::floatfield);
	log << "Ligand,nConfs,idock score (kcal/mol),RF-Score (pKd)" << endl << setprecision(2);
	// Start to dock each input ligand.
	size_t index = 0;
	for (const auto& input_ligand_path : input_ligand_paths)
	{
		// Output the ligand file stem.
		const string stem = input_ligand_path.stem().string();
		cout << setw(8) << ++index << "   " << setw(16) << stem << "   " << flush;

		// Check if the current ligand has already been docked.
		size_t num_confs = 0;
		double id_score = 0;
		double rf_score = 0;
		const path output_ligand_path = out_path / input_ligand_path.filename();
		if (exists(output_ligand_path) && !equivalent(ligand_path, out_path) && !anna_test)
		{
			// Extract idock score and RF-Score from output file.
			// cout << "Use previous dock" << endl;
			// string line;
			// for (ifstream ifs(output_ligand_path); getline(ifs, line);)
			// {
			// 	const string record = line.substr(0, 10);
			// 	if (record == "MODEL     ")
			// 	{
			// 		++num_confs;
			// 	}
			// 	else if (num_confs == 1 && record == "REMARK 921")
			// 	{
			// 		id_score = stod(line.substr(55, 8));
			// 	}
			// 	else if (num_confs == 1 && record == "REMARK 927")
			// 	{
			// 		rf_score = stod(line.substr(55, 8));
			// 	}
			// }
		}
		else
		{
			// Parse the ligand.
			array<double, 3> origin;
			const ligand lig(input_ligand_path, origin);

			// Find atom types that are present in the current ligand but not present in the grid maps.
			vector<size_t> xs;
			for (size_t t = 0; t < sf.n; ++t)
			{
				if (lig.xs[t] && rec.maps[t].empty())
				{
					rec.maps[t].resize(rec.num_probes_product);
					xs.push_back(t);
				}
			}

			// Create grid maps on the fly if necessary.
			if (xs.size())
			{
				// Precalculate p_offset.
				rec.precalculate(xs);

				// Populate the grid map task container.
				cnt.init(rec.num_probes[2]);
				for (size_t z = 0; z < rec.num_probes[2]; ++z)
				{
					io.post([&, z]()
					{
						rec.populate(xs, z, sf);
						cnt.increment();
					});
				}
				cnt.wait();
			}

			if (score_only)
			{
				cout << "SCORE ONLY" <<endl;
				if (anna_test){
					cout << "ANNA TEST" << endl;
					ofstream testfile;
					testfile.open("test_03.txt");

					// Define constants.
					static const double pi = 3.1415926535897932; //!< Pi.
					static const size_t seed = 1641317389;
					const size_t num_entities = 2 + lig.num_active_torsions; // Number of entities to mutate.
					const double e_upper_bound = static_cast<double>(4 * lig.num_heavy_atoms); // A conformation will be droped if its free energy is not better than e_upper_bound.
					cout << "the e_upper_bound" << e_upper_bound << endl;
					mt19937_64 rng(seed);
					uniform_real_distribution<double> u01(0, 1);
					uniform_real_distribution<double> u11(-1, 1);
					uniform_real_distribution<double> upi(-pi, pi);
					uniform_real_distribution<double> ub0(rec.corner0[0], rec.corner1[0]);
					uniform_real_distribution<double> ub1(rec.corner0[1], rec.corner1[1]);
					uniform_real_distribution<double> ub2(rec.corner0[2], rec.corner1[2]);
					uniform_int_distribution<size_t> uen(0, num_entities - 1);
					normal_distribution<double> n01(0, 1);

					// Generate an initial random conformation c0, and evaluate it.
					conformation c0(lig.num_active_torsions);
					double e0, f0;
					change g0(lig.num_active_torsions);
					bool valid_conformation = false;
					for (size_t i = 0; (i < 1000); ++i)
					{
						// Randomize conformation c0.
						c0.position = array<double, 3>{{ub0(rng), ub1(rng), ub2(rng)}};
						for (size_t i = 0; i < c0.position.size(); i++)
						{
							//cout << c0.position[i] << " ";
							testfile << c0.position[i] << " ";
						}
						c0.orientation = normalize(array<double, 4>{{n01(rng), n01(rng), n01(rng), n01(rng)}});
						for (size_t i = 0; i < c0.orientation.size(); i++)
						{
							//cout << c0.orientation[i] << " ";
							testfile << c0.orientation[i] << " ";
						}
						for (size_t i = 0; i < lig.num_active_torsions; ++i)
						{
							c0.torsions[i] = upi(rng);
							//cout << c0.torsions[i] << " ";
							testfile << c0.torsions[i] << " ";
						}
						if(lig.evaluate(c0, sf, rec, e_upper_bound, e0, f0, g0)){
							// Save gradient
							testfile << g0[0] << " ";
							testfile << g0[1] << " ";
							testfile << g0[2] << " ";
							testfile << g0[3] << " ";
							testfile << g0[4] << " ";
							testfile << g0[5] << " ";

							// Save the inter-molecular energy
							testfile << f0 << " ";
							// Save the intermolecular free energy
							cout << "Energy 2: " << e0 << endl;
							// Save the total free energy
							testfile << e0 << endl;

						}
						else
						{
							testfile << 0 << endl;
						}

					}
					testfile.close();
				}
				else{
				num_confs = 1;
				conformation c0(lig.num_active_torsions);
				c0.position = origin;
				double e0, f0;
				change g0(0);
				lig.evaluate(c0, sf, rec, -99, e0, f0, g0);
				auto r0 = lig.compose_result(e0, f0, c0);
				r0.e_nd = r0.f * lig.flexibility_penalty_factor;
				r0.rf = lig.calculate_rf_score(r0, rec, f);
				id_score = r0.e_nd;
				rf_score = r0.rf;
				lig.write_models(output_ligand_path, {{ move(r0) }}, rec);
				}
			}
			else
			{
				// Run the Monte Carlo tasks.
				cnt.init(num_tasks);
				for (size_t i = 0; i < num_tasks; ++i)
				{
					assert(result_containers[i].empty());
					const size_t s = rng();
					io.post([&, i, s]()
					{
						lig.monte_carlo(result_containers[i], s, sf, rec);
						cnt.increment();
					});
				}
				cnt.wait();

				// Merge results from all tasks into one single result container.
				assert(results.empty());
				const double required_square_error = static_cast<double>(4 * lig.num_heavy_atoms); // Ligands with RMSD < 2.0 will be clustered into the same cluster.
				for (auto& result_container : result_containers)
				{
					for (auto& result : result_container)
					{
						result::push(results, move(result), required_square_error);
					}
					result_container.clear();
				}

				// If conformations are found, output them.
				num_confs = results.size();
				if (num_confs)
				{
					// Adjust free energy relative to the best conformation and flexibility.
					const auto& best_result = results.front();
					const double best_result_intra_e = best_result.e - best_result.f;
					for (auto& result : results)
					{
						result.e_nd = (result.e - best_result_intra_e) * lig.flexibility_penalty_factor;
						result.rf = lig.calculate_rf_score(result, rec, f);
					}
					id_score = best_result.e_nd;
					rf_score = best_result.rf;

					// Write models to file.
					lig.write_models(output_ligand_path, results, rec);

					// Clear the results of the current ligand.
					results.clear();
				}
			}
		}

		// If output file or conformations are found, output the idock score and RF-Score.
		cout << setw(6) << num_confs;
		log << stem << ',' << num_confs;
		if (num_confs)
		{
			cout << "   " << setw(22) << id_score << "   " << setw(14) << rf_score;
			log << ',' << id_score << ',' << rf_score;
		}
		cout << endl;
		log << '\n';

		// Output to the log file in csv format. The log file can be sorted using: head -1 log.csv && tail -n +2 log.csv | awk -F, '{ printf "%s,%s\n", $2||0, $0 }' | sort -t, -k1nr -k3n | cut -d, -f2-
	}

	// Wait until the io service pool has finished all its tasks.
	io.wait();
}


//
// Created by Andrea on 28/07/2017.
//

#include "Task3D.h"

#include <custom_conversions/Conversions.h>
#include <vtkCubeSource.h>
#include <boost/thread/thread.hpp>

namespace  ThreeDColors {
    double Green_Arrow[3]{0.3176, 0.6431, 0.3215};
    double Green[3]{0.0, 0.9, 0.03};
    double Yellow[3]{1.0, 0.702, 0.0};

}
Task3D::Task3D()
        :
        SimTask(NULL,100) ,
        time_last(ros::Time::now())
{

    InitBullet();

    double density = 120000;
    double friction = 0.5;
    std::vector<int> first_path = {1, 3, 2, 0};
    std::vector<int> second_path = {3, 1, 0, 2};
    std::vector<int> third_path = {2, 1, 0, 3};
    index.push_back(first_path);
    index.push_back(second_path);
    index.push_back(third_path);
    target = std::rand() % 3;
    task_state = TaskState::Idle;

    // -------------------------------------------------------------------------
    // Create static target objects

    radii = new double[rings_number] {0.012, 0.01, 0.011, 0.014};

    std::vector<double> _dim = {1};

    rot.DoRotX(M_PI);
    rot.DoRotZ(-M_PI/180*105);

    KDL::Vector cam_position(0.118884, 0.27565, 0.14583);
    KDL::Vector focal_point(-0.04443, -0.57915, -0.413638);
    KDL::Vector direction = (focal_point-cam_position);
    direction = direction/direction.Norm();

    kine_dim = {0.002, 0.002};

    for (int i = 0; i<rings_number; i++){

        std::stringstream input_file_dir;
        input_file_dir << MESH_DIRECTORY << std::string("3Dring")
                       << i+1 <<std::string(".obj");
        std::string mesh_file_dir_str = input_file_dir.str();

        KDL::Vector ring_pos;
        ring_pos = cam_position +  ((double)i* 0.05 + 0.16 +
                                    sin((double)i*M_PI/3)*(0.05/4))*direction;
        KDL::Frame pose(rot,KDL::Vector(ring_pos.x(),
                                        ring_pos.y(),
                                        ring_pos.z()) );
        ideal_position[i].x(pose.p[0]);
        ideal_position[i].y(pose.p[1]);
        ideal_position[i].z(pose.p[2]);

        ring[i] = new SimObject(ObjectShape::MESH, ObjectType::DYNAMIC, _dim,
                                pose, density, friction,
                                mesh_file_dir_str, 0);

        dynamicsWorld->addRigidBody(ring[i]->GetBody());
        graphics_actors.push_back(ring[i]->GetActor());
        ring[i]->GetActor()->GetProperty()->SetColor(0.8f, 0.8f, 0.8f);

        const btVector3 btPivotA(0.f, 0.f, -float(0.025f+radii[i])*B_DIM_SCALE);
        btVector3 btAxisA( 1.0f, 0.0f, 0.0f );
        hinges[i] = new btHingeConstraint( *ring[i]->GetBody(), btPivotA, btAxisA );
        hinges[i]->enableAngularMotor(true, 0 , 0.00015);
        dynamicsWorld->addConstraint(hinges[i]);

        // ---------------------------------------------------------------------
        // hinge objects

        std::stringstream input_file_dir_hinge;
        input_file_dir_hinge << MESH_DIRECTORY << std::string("hinge")
                             <<std::string(".obj");
        std::string mesh_file_dir_hinge_str = input_file_dir_hinge.str();

        KDL::Frame pose2(rot,KDL::Vector(ring_pos.x(),
                                        ring_pos.y(),
                                        ring_pos.z() + 0.025 + radii[i]) );
        hinge_cyl[i] = new SimObject(ObjectShape::MESH, ObjectType::DYNAMIC,
                                     _dim, pose2, 0.0, friction,
                                     mesh_file_dir_hinge_str, 0);

        dynamicsWorld->addRigidBody(hinge_cyl[i]->GetBody());
        hinge_cyl[i]->GetActor()->GetProperty()->SetColor(0.4, 0.4, 0.4);
        graphics_actors.push_back(hinge_cyl[i]->GetActor());
    }

    // -------------------------------------------------------------------------
    // Arrow for Idle

    std::stringstream input_file_dir;
    input_file_dir << MESH_DIRECTORY << std::string("arrow")
                   <<std::string(".obj");
    std::string mesh_file_dir_str = input_file_dir.str();

    rot.DoRotX(-M_PI/2);
    rot.GetQuaternion(arrow_x, arrow_y, arrow_z, arrow_w);

    arrow = new SimObject(ObjectShape::MESH, ObjectType::DYNAMIC, _dim,
                          KDL::Frame(), 0.0, friction,
                          mesh_file_dir_str, 0);

    dynamicsWorld->addRigidBody(arrow->GetBody());
    graphics_actors.push_back(arrow->GetActor());
    arrow->GetActor()->GetProperty()->SetColor(ThreeDColors::Green_Arrow);
    arrow->GetActor()->GetProperty()->SetOpacity(1);

    // -------------------------------------------------------------------------
    // Create kinematic pointer

    friction = 0.1;

    kine_dim = {0.005, 4*0.007};
    kine_p=
            new SimObject(ObjectShape::CYLINDER, ObjectType::KINEMATIC,
                          kine_dim, KDL::Frame(), 0.0, friction,
                          NULL, 0);
    dynamicsWorld->addRigidBody(kine_p->GetBody());
    kine_p->GetActor()->GetProperty()->SetColor(0.6314, 0.0, 0.0);
    graphics_actors.push_back(kine_p->GetActor());

    // Define a master manipulator
    master = new Manipulator(nh, "/sigma7/sigma0", "/pose", "/gripper_angle");
};

//------------------------------------------------------------------------------
void Task3D::StepWorld() {

    //-----------------POINTER: update position on the upper plane

    KDL::Frame tool_pose;

    master->GetPoseWorld(tool_pose);

    pointer_posit = tool_pose * KDL::Vector( -0.0, -0.0, -0.03+0.01);
    KDL::Rotation _rot;
    _rot.DoRotZ(M_PI/180*105);
    double x, y, z, w;
    _rot.GetQuaternion(x, y, z, w);
    double pointer_pose[7] = {pointer_posit[0], pointer_posit[1], pointer_posit[2],
                              x,y,z,w};
    kine_p->SetKinematicPose(pointer_pose);

    //--------------------------------
    // step the world
    StepPhysics();

    if (task_state == TaskState::Idle){
        // Manage the position of the arrow
        ArrowManager();
        axes_dist=0;
    }
    else{
        // Find the tool coordinates in the target rf
        vtkMatrix4x4* ring_rf = vtkMatrix4x4::New();
        ring[index[target][path]]->GetActor()->GetMatrix(ring_rf);
        double actual_posit[4] = {pointer_posit[0], pointer_posit[1],
                                  pointer_posit[2], 1};
        ring_rf->Invert();
        ring_rf->MultiplyPoint(actual_posit, transf);

        if (task_state == TaskState::Entry){
            // Verify if the tool is crossing the target
            CheckCrossing();
        }
        else if (task_state == TaskState::Exit){

            ExitChecking();
        }

        TaskEvaluation();

        // Compute the distance between the cylinder axis and the axis
        // orthogonal to the ring and passing through its center
        axes_dist=(float) sqrt(pow(transf[0], 2) + pow(transf[2], 2));

        // Verify if the ring is touched
        if (fabs(ring[index[target][path]]->GetActor()->GetCenter()[0] -
                 ideal_position[index[target][path]].x()) >= 0.0005){
            touch = 1;
        }

    }
}

//------------------------------------------------------------------------------
void Task3D::ArrowManager() {

    ring[index[target][path]]->GetActor()->GetProperty()->SetColor
            (ThreeDColors::Yellow);

    KDL::Vector shift(0.0, 0.0, -0.03 + 0.005*sin(var));
    arrow_posit = rot*shift;

    double* pose;
    pose = new double[7]{arrow_posit[0] + ideal_position[index[target][path]].x(),
                         arrow_posit[1] + ideal_position[index[target][path]].y(),
                         arrow_posit[2] + ideal_position[index[target][path]].z(),
                         arrow_x, arrow_y, arrow_z, arrow_w};
    var = var + 0.05;

    arrow->GetActor()->SetUserMatrix(PoseArrayToVTKMatrix(pose));

    // Check if the arrow has been approached
    double* arrow_position;
    arrow_position = arrow->GetActor()->GetCenter();
    KDL::Vector arrow_pos;
    arrow_pos.x(arrow_position[0]);
    arrow_pos.y(arrow_position[1]);
    arrow_pos.z(arrow_position[2]);
    distance = arrow_pos - pointer_posit;
    double norm_distance = distance.Norm();
    if (norm_distance <= threshold) {
        task_state = TaskState::Entry;
        begin = ros::Time::now();
        arrow->GetActor()->GetProperty()->SetOpacity(0.0);
        var = 0;
    }
}

//------------------------------------------------------------------------------
void Task3D::CheckCrossing() {

    // Verify if the tool is crossing the target
    if (transf[1] >= -kine_dim[1]/2){
        cond=1;
    }

    // Verify if the tool is crossing the target
    if (transf[1] >= 0 && transf[1] <= kine_dim[1]/2 &&
        pow(transf[0], 2) + pow(transf[2], 2) <= pow
                (radii[index[target][path]], 2)){

        ring[index[target][path]]->GetActor()->GetProperty()->SetColor
                (ThreeDColors::Green);
        task_state = TaskState::Exit;
        cond=0;
    }

}

//------------------------------------------------------------------------------
void Task3D::TaskEvaluation() {

    // Populate the task state message
    task_state_msg.task_name = "3D Task";
    task_state_msg.task_state = (uint8_t)task_state;
    task_state_msg.number_of_repetition = n_rep;

    // ******* TO FINISH!!!!!!!!!! *******
    //task_state_msg.error_condition=cond;
    //task_state_msg.ring_touch=touch;
    //************************************

    task_state_msg.error_field_1 = axes_dist;

    time = (ros::Time::now() - begin).toSec();
    task_state_msg.time_stamp = time;
}

//------------------------------------------------------------------------------
void Task3D::ExitChecking() {

    if (transf[1] >= -kine_dim[1]/2) {
        if (path == 3){
            ring[index[target][path]]->GetActor()->GetProperty()->SetColor
                    (ThreeDColors::Green);
        }
        else {
            ring[index[target][path
                               + 1]]->GetActor()->GetProperty()->SetColor
                    (ThreeDColors::Yellow);
            ring[index[target][path]]->GetActor()->GetProperty()->SetColor(ThreeDColors::Green);
        }
    }
    if (transf[1] < -kine_dim[1]/2){
        ring[index[target][path]]->GetActor()->GetProperty()->SetColor(1.0, 1.0, 1.0);
        path = path + 1;
        task_state = TaskState::Entry;
        touch = 0;
        if (path > 3){
            target = std::rand() % 3;
            path = 0;
            task_state = TaskState::Idle;
            n_rep=n_rep+(unsigned char)1;
            arrow->GetActor()->GetProperty()->SetOpacity(1);
        }
    }
}

custom_msgs::TaskState Task3D::GetTaskStateMsg() {
    return task_state_msg;
}

void Task3D::ResetTask() {
    ROS_INFO("Repetition completed. Resetting the task.");

}

void Task3D::ResetCurrentAcquisition() {
    ROS_INFO("Resetting current acquisition.");

}

void Task3D::HapticsThread() {

    ros::Publisher pub_desired[2];

    ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
    pub_desired[0] = node->advertise<geometry_msgs::PoseStamped>
            ("/PSM1/tool_pose_desired", 10);
//    if(bimanual)
//        pub_desired[1] = node->advertise<geometry_msgs::PoseStamped>
//                ("/PSM2/tool_pose_desired", 10);

    ros::Rate loop_rate(200);

    while (ros::ok())
    {

//        CalculatedDesiredToolPose();

//        // publish desired poses
//        for (int n_arm = 0; n_arm < 1+ int(bimanual); ++n_arm) {
//
//            // convert to pose message
//            geometry_msgs::PoseStamped pose_msg;
//            tf::poseKDLToMsg(tool_desired_pose_kdl[n_arm], pose_msg.pose);
//            // fill the header
//            pose_msg.header.frame_id = "/task_space";
//            pose_msg.header.stamp = ros::Time::now();
//            // publish
//            pub_desired[n_arm].publish(pose_msg);
//        }

        ros::spinOnce();
        loop_rate.sleep();
        boost::this_thread::interruption_point();
    }
}

void Task3D::InitBullet() {

    ///-----initialization_start-----

    ///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
    collisionConfiguration = new btDefaultCollisionConfiguration();

    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    dispatcher = new btCollisionDispatcher(collisionConfiguration);

    ///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
    overlappingPairCache = new btDbvtBroadphase();

    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    solver = new btSequentialImpulseConstraintSolver;

    dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,
                                                overlappingPairCache, solver,
                                                collisionConfiguration);

    dynamicsWorld->setGravity(btVector3(0, 0, -10));

}

void Task3D::StepPhysics() {
    ///-----stepsimulation_start-----

    ///-----stepsimulation_start-----
    double time_step = (ros::Time::now() - time_last).toSec();

    // simulation seems more realistic when time_step is halved right now!
    dynamicsWorld->stepSimulation(btScalar(time_step), 5);
    time_last = ros::Time::now();
//    //print positions of all objects
//    for (int j = dynamics_world->getNumCollisionObjects() - 1; j >= 0; j--)
//    {
//        btCollisionObject* obj = dynamics_world->getCollisionObjectArray()[j];
//        btRigidBody* body_ = btRigidBody::upcast(obj);
//        btTransform trans;
//        if (body_ && body_->getMotionState())
//        {
//            body_->getMotionState()->getWorldTransform(trans);
//        }
//        else
//        {
//            trans = obj->getWorldTransform();
//        }
//
//            heights[j] = trans.getOrigin().z();
//        heights2[j] = graphics_actors[j]->GetMatrix()->Element[2][3];
//    }

}

Task3D::~Task3D() {

    ROS_INFO("Destructing Bullet task: %d",
             dynamicsWorld->getNumCollisionObjects());
    //remove the rigidbodies from the dynamics world and delete them
    for (int i = dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
    {
        btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i];
        btRigidBody* body = btRigidBody::upcast(obj);
        if (body && body->getMotionState())
        {
            delete body->getMotionState();
        }
        dynamicsWorld->removeCollisionObject(obj);
        delete obj;
    }

    for (int j = 0; j < rings_number; ++j) {

        delete hinges[j];
    }
//    for (int j = 0; j < NUM_BULLET_SPHERES; ++j) {
//        SimObject* sphere = spheres[j];
//        spheres[j] = 0;
//        delete sphere;
//    }
//    //delete collision shapes
////    for (int j = 0; j < collisionShapes.size(); j++)
//    for (int j = 0; j < 2; j++) // because we use the same collision shape
//        // for all spheres
//    {
//        btCollisionShape* shape = collisionShapes[j];
//        collisionShapes[j] = 0;
//        delete shape;
//    }

    //delete dynamics world
    delete dynamicsWorld;

    //delete solver
    delete solver;

    //delete broadphase
    delete overlappingPairCache;

    //delete dispatcher
    delete dispatcher;

    delete collisionConfiguration;

    //next line is optional: it will be cleared by the destructor when the array goes out of scope
//    collisionShapes.clear();
}


#include "settings_utils.h"

#include <boost/any.hpp>
#include <boost/program_options.hpp>
#include <boost/spirit/include/qi.hpp>

namespace carl::settings {

// Adapted from https://stackoverflow.com/questions/45071699/
void validate(boost::any& v, const std::vector<std::string>& values, carl::settings::duration* /*unused*/, int /*unused*/) {
	namespace pov = boost::program_options::validators;
	namespace qi = boost::spirit::qi;

	std::string s(pov::get_single_string(values));

	long value = 0;
	std::chrono::nanoseconds factor;
	qi::symbols<char, std::chrono::nanoseconds> unit;
	unit.add
		("ns", std::chrono::nanoseconds(1))
		("µs", std::chrono::microseconds(1))
		("us", std::chrono::microseconds(1))
		("ms", std::chrono::milliseconds(1))
		("s", std::chrono::seconds(1))
		("m", std::chrono::minutes(1))
		("h", std::chrono::hours(1))
	;
	if (qi::parse(s.begin(), s.end(), qi::long_ >> unit >> qi::eoi, value, factor)) {
		v = duration(value * factor);
	} else {
		throw boost::program_options::invalid_option_value(s);
	}
}

void validate(boost::any& v, const std::vector<std::string>& values, carl::settings::binary_quantity* /*unused*/, int /*unused*/) {
	namespace pov = boost::program_options::validators;
	namespace qi = boost::spirit::qi;

	std::string s(pov::get_single_string(values));

	std::size_t value = 0;
	std::size_t factor = 1;
	qi::symbols<char, std::size_t> unit;
	unit.add
		("K", static_cast<std::size_t>(1) << 10U)
		("Ki", static_cast<std::size_t>(1) << 10U)
		("M", static_cast<std::size_t>(1) << 20U)
		("Mi", static_cast<std::size_t>(1) << 20U)
		("G", static_cast<std::size_t>(1) << 30U)
		("Gi", static_cast<std::size_t>(1) << 30U)
		("T", static_cast<std::size_t>(1) << 40U)
		("Ti", static_cast<std::size_t>(1) << 40U)
		("P", static_cast<std::size_t>(1) << 50U)
		("Pi", static_cast<std::size_t>(1) << 50U)
		("E", static_cast<std::size_t>(1) << 60U)
		("Ei", static_cast<std::size_t>(1) << 60U)
	;
	if (qi::parse(s.begin(), s.end(), qi::ulong_long >> -unit >> qi::eoi, value, factor)) {
		v = binary_quantity(value * factor);
	} else {
		throw boost::program_options::invalid_option_value(s);
	}
}

void validate(boost::any& v, const std::vector<std::string>& values, carl::settings::metric_quantity* /*unused*/, int /*unused*/) {
	namespace pov = boost::program_options::validators;
	namespace qi = boost::spirit::qi;

	std::string s(pov::get_single_string(values));

	std::size_t value = 0;
	std::size_t factor = 1;
	qi::symbols<char, std::size_t> unit;
	unit.add
		("K", 1000)
		("M", 1000000)
		("G", 1000000000)
		("T", 1000000000000)
		("P", 1000000000000000)
		("E", 1000000000000000000)
	;
	if (qi::parse(s.begin(), s.end(), qi::ulong_long >> -unit >> qi::eoi, value, factor)) {
		v = metric_quantity(value * factor);
	} else {
		throw boost::program_options::invalid_option_value(s);
	}
}

}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <memory>
#include <pup.h>
#include <string>
#include <utility>

#include "DataStructures/DataBox/DataBox.hpp"
#include "DataStructures/DataBox/Tag.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/CoordinateMaps/Affine.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.tpp"
#include "Domain/CoordinateMaps/ProductMaps.hpp"
#include "Domain/CoordinateMaps/ProductMaps.tpp"
#include "Domain/CoordinateMaps/TimeDependent/CubicScale.hpp"
#include "Domain/ElementMap.hpp"
#include "Domain/FunctionsOfTime/FunctionOfTime.hpp"
#include "Domain/FunctionsOfTime/PiecewisePolynomial.hpp"
#include "Domain/FunctionsOfTime/RegisterDerivedWithCharm.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/SizeOfElement.hpp"  // IWYU pragma: keep
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Tags.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/LimiterActions.hpp"  // IWYU pragma: keep
#include "Evolution/DiscontinuousGalerkin/Limiters/Minmod.tpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodType.hpp"
#include "Framework/ActionTesting.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Parallel/PhaseDependentActionList.hpp"  // IWYU pragma: keep
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

// IWYU pragma: no_include "Evolution/DiscontinuousGalerkin/Limiters/Minmod.hpp"

// IWYU pragma: no_forward_declare ActionTesting::InitializeDataBox

namespace {
struct TemporalId : db::SimpleTag {
  using type = int;
};

struct Var : db::SimpleTag {
  using type = Scalar<DataVector>;
};

template <size_t Dim>
struct System {
  static constexpr const size_t volume_dim = Dim;
  using variables_tag = Tags::Variables<tmpl::list<Var>>;
};

struct LimiterTag : db::SimpleTag {
  using type = Limiters::Minmod<2, tmpl::list<Var>>;
};

template <size_t Dim, typename Metavariables>
struct component {
  using metavariables = Metavariables;
  using chare_type = ActionTesting::MockArrayChare;
  using array_index = ElementId<Dim>;
  using const_global_cache_tags = tmpl::list<LimiterTag>;
  using simple_tags = db::AddSimpleTags<
      TemporalId, domain::Tags::Mesh<Dim>, domain::Tags::Element<Dim>,
      domain::Tags::ElementMap<Dim, Frame::Grid>,
      domain::CoordinateMaps::Tags::CoordinateMap<2, Frame::Grid,
                                                  Frame::Inertial>,
      ::Tags::Time, domain::Tags::FunctionsOfTime, Var>;
  using compute_tags =
      db::AddComputeTags<::domain::Tags::LogicalCoordinates<Dim>,
                         ::domain::Tags::MappedCoordinates<
                             ::domain::Tags::ElementMap<Dim, Frame::Grid>,
                             ::domain::Tags::Coordinates<Dim, Frame::Logical>>,
                         domain::Tags::SizeOfElementCompute<Dim>>;
  using phase_dependent_action_list = tmpl::list<
      Parallel::PhaseActions<
          typename Metavariables::Phase, Metavariables::Phase::Initialization,
          tmpl::list<
              ActionTesting::InitializeDataBox<simple_tags, compute_tags>>>,
      Parallel::PhaseActions<
          typename Metavariables::Phase, Metavariables::Phase::Testing,
          tmpl::list<Limiters::Actions::SendData<Metavariables>,
                     Limiters::Actions::Limit<Metavariables>>>>;
};

template <size_t Dim>
struct Metavariables {
  using component_list = tmpl::list<component<Dim, Metavariables>>;
  using limiter = LimiterTag;
  using system = System<Dim>;
  using temporal_id = TemporalId;
  static constexpr bool local_time_stepping = false;
  enum class Phase { Initialization, Testing, Exit };
};
}  // namespace

// This test checks that the Minmod limiter's interfaces and type aliases
// succesfully integrate with the limiter actions. It does this by compiling
// together the Minmod limiter and the actions, then making calls to the
// SendData and the Limit actions. No checks are performed here that the limiter
// and/or actions produce correct output: that is done in other tests.
SPECTRE_TEST_CASE("Unit.Evolution.DG.Limiters.LimiterActions.Minmod",
                  "[Unit][NumericalAlgorithms][Actions]") {
  using metavariables = Metavariables<2>;
  using my_component = component<2, metavariables>;

  const Mesh<2> mesh{3, Spectral::Basis::Legendre,
                     Spectral::Quadrature::GaussLobatto};
  const ElementId<2> self_id(1, {{{2, 0}, {1, 0}}});
  const Element<2> element(self_id, {});

  using Affine = domain::CoordinateMaps::Affine;
  using Affine2D = domain::CoordinateMaps::ProductOf2Maps<Affine, Affine>;
  using CubicScaleMap = domain::CoordinateMaps::TimeDependent::CubicScale<2>;
  PUPable_reg(
      SINGLE_ARG(domain::CoordinateMap<Frame::Logical, Frame::Grid, Affine2D>));
  PUPable_reg(SINGLE_ARG(
      domain::CoordinateMap<Frame::Grid, Frame::Inertial, CubicScaleMap>));
  domain::FunctionsOfTime::register_derived_with_charm();

  const Affine xi_map{-1., 1., 3., 7.};
  const Affine eta_map{-1., 1., 7., 3.};
  auto logical_to_grid_map = ElementMap<2, Frame::Grid>(
      self_id, domain::make_coordinate_map_base<Frame::Logical, Frame::Grid>(
                   Affine2D(xi_map, eta_map)));
  std::unique_ptr<domain::CoordinateMapBase<Frame::Grid, Frame::Inertial, 2>>
      grid_to_inertial_map =
          domain::make_coordinate_map_base<Frame::Grid, Frame::Inertial>(
              CubicScaleMap{10.0, "Expansion", "Expansion"});

  const double initial_time = 0.0;
  const double expiration_time = 2.5;
  std::unordered_map<std::string,
                     std::unique_ptr<domain::FunctionsOfTime::FunctionOfTime>>
      functions_of_time{};
  functions_of_time.insert(std::make_pair(
      "Expansion",
      std::make_unique<domain::FunctionsOfTime::PiecewisePolynomial<2>>(
          initial_time, std::array<DataVector, 3>{{{0.0}, {1.0}, {0.0}}},
          expiration_time)));

  auto var = Scalar<DataVector>(mesh.number_of_grid_points(), 1234.);

  const double tvb_constant = 0.0;
  ActionTesting::MockRuntimeSystem<metavariables> runner{
      Limiters::Minmod<2, tmpl::list<Var>>(Limiters::MinmodType::LambdaPi1,
                                           tvb_constant)};
  ActionTesting::emplace_component_and_initialize<my_component>(
      &runner, self_id,
      {0, mesh, element, std::move(logical_to_grid_map),
       std::move(grid_to_inertial_map), 1.0, std::move(functions_of_time),
       std::move(var)});
  ActionTesting::set_phase(make_not_null(&runner),
                           metavariables::Phase::Testing);

  // SendData
  runner.next_action<my_component>(self_id);
  CHECK(runner.is_ready<my_component>(self_id));
  // Limit
  runner.next_action<my_component>(self_id);
}


// Copyright (c) 2013, German Neuroinformatics Node (G-Node)
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted under the terms of the BSD License. See
// LICENSE file in the root of the Project.

#ifndef NIX_CHECKS_H
#define NIX_CHECKS_H

#include <nix/util/util.hpp>
#include <nix/valid/helper.hpp>

#include <nix/base/IDimensions.hpp>

#include <nix/types.hpp>

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

namespace nix {
namespace valid {

    /**
     * @brief Check if later given not greater than initally defined value.
     *
     * One Check struct that checks whether the given value is not greater
     * than the initially given other value, both of which have to be
     * convertible to double.
     */
    struct notGreater {
        const double value;

        template<typename T>
        notGreater(T value) : value(static_cast<double>(value)) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return !(static_cast<double>(val) > value);
        }
    };

    /**
     * @brief Check if later given greater than initally defined value.
     *
     * One Check struct that checks whether the given value is greater than
     * the initially given other value, both of which have to be
     * convertible to double.
     */
    struct isGreater {
        const double value;

        template<typename T>
        isGreater(T value) : value(static_cast<double>(value)) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return static_cast<double>(val) > value;
        }
    };

    /**
     * @brief Check if later given not smaller than initally defined value.
     *
     * One Check struct that checks whether the given value is not smaller
     * than the initially given other value, both of which have to be
     * convertible to double.
     */
    struct notSmaller {
        const double value;

        template<typename T>
        notSmaller(T value) : value(static_cast<double>(value)) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return !(static_cast<double>(val) < value);
        }
    };

    /**
     * @brief Check if later given smaller than initally defined value.
     *
     * One Check struct that checks whether the given value is smaller than
     * the initially given other value, both of which have to be
     * convertible to double.
     */
    struct isSmaller {
        const double value;

        template<typename T>
        isSmaller(T value) : value(static_cast<double>(value)) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return static_cast<double>(val) < value;
        }
    };

    /**
     * @brief Check for un-equality of initally defined and later given value.
     *
     * One Check struct that checks whether the given value is not equal
     * to the initially given other value.
     */
    template<typename T>
    struct notEqual {
        const T value;

        notEqual(T value) : value(value) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return value != val;
        }
    };

    /**
     * @brief Check for equality of initally defined and later given value.
     *
     * One Check struct that checks whether the given value is equal to
     * the initially given other value.
     */
    template<typename T>
    struct isEqual {
        const T value;

        isEqual(T value) : value(value) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return value == val;
        }
    };
    // needed because: for bizarre reasons bool converts to int when compared to boost::optional
    template<>
    struct isEqual<bool> {
        const bool value;

        isEqual(bool value) : value(value) {}

        template<typename T2>
        bool operator()(const T2 &val) const {
            return value ? !!val : !val;
        }
    };

    /**
     * @brief Check if given value casts to boolean true
     *
     * One Check struct that checks whether the given value casts to true
     * or to false.
     * T can be: boost::optional, boost::none, nix-entity
     * and any basic type.
     */
    struct notFalse {
        // WARNING: enum will convert via int, which means 0 = false !
        template<typename T>
        bool operator()(const T &val) const {
            return !!val;
        }
    };

    /**
     * @brief Check if given value casts to boolean false
     *
     * One Check struct that checks whether the given value casts to false
     * or to true.
     * T can be: boost::optional, boost::none, nix-entity
     * and any basic type.
     */
    struct isFalse {
        // WARNING: enum will convert via int, which means 0 = false !
        template<typename T>
        bool operator()(const T &val) const {
            return !val;
        }
    };

    /**
     * @brief Check if given class/struct returns "empty() == false"
     *
     * One Check struct that checks whether the given value is not empty
     * or is empty.
     * T can be: any STL container.
     */
    struct notEmpty {
        template<typename T>
        bool operator()(const T &val) const {
            return !(val.empty());
        }
    };

    /**
     * @brief Check if given class/struct returns "empty() == true"
     *
     * One Check struct that checks whether the given value is empty or
     * not.
     * T can be: any STL container.
     */
    struct isEmpty {
        template<typename T>
        bool operator()(const T &val) const {
            return val.empty();
        }
    };

    /**
     * @brief Check if given class represents valid SI unit string(s)
     *
     * Base struct to be inherited by the {@link isValidUnit}, {@link
     * isAtomicUnit}, {@link isCompoundUnit}. Not viable on its own!
     */
    struct isUnit {
        typedef std::function<bool(std::string)> TPRED;

        virtual bool operator()(const std::string &u) const = 0;

        bool operator()(const boost::optional<std::string> &u) const {
            // note: relying on short-curcuiting here
            return u && (*this)(*u);
        }

        bool operator()(const std::vector<std::string> &u, TPRED obj) const {
            // if test succeeds find_if_not will not find anything & return it == end
            return std::find_if_not(u.begin(), u.end(), obj) == u.end();
        }

        virtual ~isUnit() { }
    };

    /**
     * @brief Check if given class represents valid SI unit string(s)
     *
     * One Check struct that checks whether the given string(s) represent(s)
     * a valid atomic or compound SI unit.
     * Parameter can be of type boost optional (containing nothing or
     * string) or of type string or a vector of strings.
     */
    struct isValidUnit : public isUnit {
        bool operator()(const std::string &u) const {
            return (util::isSIUnit(u) || util::isCompoundSIUnit(u));
        }
        bool operator()(const boost::optional<std::string> &u) const {
            return isUnit::operator()(u);
        }
        bool operator()(const std::vector<std::string> &u) const {
            return isUnit::operator()(u, *this);
        }
    };

    /**
     * @brief Check if given class represents valid atomic SI unit string(s)
     *
     * One Check struct that checks whether the given string(s) represent(s)
     * a valid atomic SI unit.
     * Parameter can be of type boost optional (containing nothing or
     * string) or of type string or a vector of strings.
     */
    struct isAtomicUnit : public isUnit {
        bool operator()(const std::string &u) const {
            return util::isSIUnit(u);
        }
        bool operator()(const boost::optional<std::string> &u) const {
            return isUnit::operator()(u);
        }
        bool operator()(const std::vector<std::string> &u) const {
            return isUnit::operator()(u, *this);
        }
    };

    /**
     * @brief Check if given class represents valid compound SI unit string(s)
     *
     * One Check struct that checks whether the given string(s) represent(s)
     * a valid compound SI unit.
     * Parameter can be of type boost optional (containing nothing or
     * string) or of type string or a vector of strings.
     */
    struct isCompoundUnit : public isUnit {
        bool operator()(const std::string &u) const {
            return util::isCompoundSIUnit(u);
        }
        bool operator()(const boost::optional<std::string> &u) const {
            return isUnit::operator()(u);
        }
        bool operator()(const std::vector<std::string> &u) const {
            return isUnit::operator()(u, *this);
        }
    };

    /**
     * @brief Check if given value can be regarded as being set
     *
     * One Check struct that checks whether the given value can be
     * considered set, by applying {@link notFalse} and {@link notEmpty}
     * checks. Value thus is set if: STL cotnainer not empty OR
     * bool is true OR boost optional is set OR number is not 0.
     * Parameter can be of above types or even boost none_t.
     * NOTE: use this if you don't know wheter a type has and "empty"
     * method.
     */
    struct isSet {
        template<typename T>
        bool operator()(const T &val) const {
            typedef typename std::conditional<hasEmpty<T>::value, notEmpty, notFalse>::type subCheck;
            return subCheck(val);
        }
    };

    /**
     * @brief Check if given container is sorted using std::is_sorted
     *
     * One Check struct that checks whether the given container is sorted
     * according to std::is_sorted. Thus supports types that are
     * supported by std::is_sorted.
     */
    struct isSorted {
        template<typename T>
        bool operator()(const T &container) const {
            return std::is_sorted(container.begin(), container.end());
        }
    };

    /**
     * @brief Check if given DataArray has given dimensionality
     *
     * One Check struct that checks whether the given DataArray entity
     * has a dimensionality of the given uint value by getting its'
     * NDSize class via the "dataExtent" method and checking its' size
     * via "size" the method.
     */
    struct NIXAPI dimEquals {
        size_t value;

        dimEquals(const size_t &value) : value(value) {}

        bool operator()(const DataArray &array) const;
    };

    /**
     * @brief Checks if units of the passed DataArray dimensions matches those
     * defined in the Tag
     *
     * Struct to check whether the units defined in the Tag match with the units
     * specified in the referenced DataArray dimensions. The check tests for
     * scalability of the tag-provided units and the ones of the DataArrays.
     *
     */
    struct NIXAPI tagUnitsMatchRefsUnits {
        std::vector<std::string> units;

        tagUnitsMatchRefsUnits(const std::vector<std::string> &units) : units(units) {}

        bool operator()(const std::vector<DataArray> &references) const;
    };

    /**
     * @brief Check if range dimension specifics ticks match data
     *
     * One Check struct that checks whether the dimensions of type
     * "Range" in the given dimensions vector have ticks that match
     * the given DataArray's data: number of ticks == number of entries
     * along the corresponding dimension in the data.
     */
    struct NIXAPI dimTicksMatchData {
        const DataArray &data;

        dimTicksMatchData(const DataArray &data) : data(data) {}

        bool operator()(const std::vector<Dimension> &dims) const;
    };

    /**
     * @brief Check if set dimension specifics labels match data
     *
     * One Check struct that checks whether the dimensions of type
     * "Set" in the given dimensions vector have labels that match
     * the given DataArray's data: number of labels == number of entries
     * along the corresponding dimension in the data.
     */
    struct NIXAPI dimLabelsMatchData {
        const DataArray &data;

        dimLabelsMatchData(const DataArray &data) : data(data) {}

        bool operator()(const std::vector<Dimension> &dims) const;
    };

    /**
     * @brief Check if DataFrame dimension specifics ticks match data
     *
     * Struct for checking whether the number of rows in the dimensions of type
     * "DataFrame" in the given dimensions vector matches the given DataArray's
     * data extent in the respective dimension: number of rows in DataFrame == number of
     * entries along the corresponding dimension in the data.
     */
    struct NIXAPI dimDataFrameTicksMatchData {
        const DataArray &data;

        dimDataFrameTicksMatchData(const DataArray &data) : data(data) {}

        bool operator()(const std::vector<Dimension> &dims) const;
    };

} // namespace valid
} // namespace nix

#endif // NIX_CHECKS_H