From 86953d60a8e798ee3d57e85eff4bf6b72fe496a0 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Mar 2021 17:51:45 +0100
Subject: [PATCH] Implement BLF_DEFINE_INTEGRATOR_STRUCTURE macro

---
 .../ContinuousDynamicalSystem/ForwardEuler.h  | 24 +++--------------
 .../ContinuousDynamicalSystem/Integrator.h    |  3 ++-
 .../ContinuousDynamicalSystem/impl/traits.h   | 27 ++++++++++++++++++-
 3 files changed, 31 insertions(+), 23 deletions(-)

diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h
index b3272f9b06..652b0caf33 100644
--- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h
+++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h
@@ -20,28 +20,10 @@ namespace BipedalLocomotion
 namespace ContinuousDynamicalSystem
 {
 template <typename _DynamicalSystem> class ForwardEuler;
+}
+}
 
-namespace internal
-{
-
-/**
- * Internal description of the ForwardEuler integrator
- */
-template <typename _DynamicalSystem> struct traits<ForwardEuler<_DynamicalSystem>>
-{
-    /** State of the integrator */
-    using State = typename traits<_DynamicalSystem>::State;
-
-    /** State derivative of the integrator */
-    using StateDerivative = typename traits<_DynamicalSystem>::StateDerivative;
-
-    /** Type of the dynamical system */
-    using DynamicalSystem = _DynamicalSystem;
-};
-
-} // namespace internal
-} // namespace ContinuousDynamicalSystem
-} // namespace BipedalLocomotion
+BLF_DEFINE_INTEGRATOR_STRUCTURE(ForwardEuler, _DynamicalSystemType)
 
 namespace BipedalLocomotion
 {
diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h
index 1fd2357be3..d6d372cbf7 100644
--- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h
+++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h
@@ -22,7 +22,8 @@ namespace ContinuousDynamicalSystem
 {
 
 /**
- * Integrator base class.
+ * Integrator base class. f$. Please inherit publicly from this class in order to define your custom
+ * integrator. Just be sure to call after your class definition #BLF_DEFINE_INTEGRATOR_STRUCTURE()
  */
 template <class _Derived> class Integrator
 {
diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h
index 0baac8e03d..919023eabf 100644
--- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h
+++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h
@@ -18,7 +18,7 @@
 #define BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE_INPUT(...) using Input = std::tuple<__VA_ARGS__>
 
 /**
- * The user must call this macro before defining a ContinuousDynamicalSystem::DynamicalSystem
+ * The user must call this macro before defining a custom ContinuousDynamicalSystem::DynamicalSystem
  * @param DynamicalSystemType the type of the dynamical system
  * @param StateType the list of the types used to define the state. The list must be defined using
  * round parenthesis. E.g. `(Eigen::VectorXd, Eigen::VectorXd)`.
@@ -47,6 +47,31 @@
     }                                                                                 \
     }
 
+/**
+ * The user must call this macro before defining a custom ContinuousDynamicalSystem::Integrator
+ * @param IntegratorType the type of the integrator.
+ * @param DynamicalSystemType the type of the dynamical system.
+ */
+#define BLF_DEFINE_INTEGRATOR_STRUCTURE(IntegratorType, DynamicalSystemType)                \
+    namespace BipedalLocomotion                                                             \
+    {                                                                                       \
+    namespace ContinuousDynamicalSystem                                                     \
+    {                                                                                       \
+    namespace internal                                                                      \
+    {                                                                                       \
+    template <class DynamicalSystemType> struct traits<IntegratorType<DynamicalSystemType>> \
+    {                                                                                       \
+        /** State of the integrator */                                                      \
+        using State = typename traits<DynamicalSystemType>::State;                          \
+        /** State derivative of the integrator */                                           \
+        using StateDerivative = typename traits<DynamicalSystemType>::StateDerivative;      \
+        /** Type of the dynamical system */                                                 \
+        using DynamicalSystem = DynamicalSystemType;                                        \
+    };                                                                                      \
+    }                                                                                       \
+    }                                                                                       \
+    }
+
 namespace BipedalLocomotion
 {
 namespace ContinuousDynamicalSystem