diff --git a/include/ruckig/position.hpp b/include/ruckig/position.hpp
index ebafa11..041fe87 100644
--- a/include/ruckig/position.hpp
+++ b/include/ruckig/position.hpp
@@ -1,6 +1,8 @@
 #pragma once
 
 #include <array>
+#include <cassert>
+#include <iterator>
 #include <optional>
 
 
@@ -105,8 +107,11 @@ class PositionSecondOrderStep1 {
     double pd;
 
     // Max 3 valid profiles
-    using ProfileIter = std::array<Profile, 3>::iterator;
-    std::array<Profile, 3> valid_profiles;
+    static constexpr int max_num_valid_profiles = 3;
+    using Profiles =  std::array<Profile, max_num_valid_profiles + 1>;
+    using ProfileIter = Profiles::iterator;
+
+    Profiles valid_profiles;
 
     void time_acc0(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, bool return_after_found) const;
     void time_none(ProfileIter& profile, double vMax, double vMin, double aMax, double aMin, bool return_after_found) const;
@@ -115,6 +120,7 @@ class PositionSecondOrderStep1 {
     bool time_all_single_step(Profile* profile, double vMax, double vMin, double aMax, double aMin) const;
 
     inline void add_profile(ProfileIter& profile) const {
+        assert(std::next(profile) != this->valid_profiles.end() && "number of valid profiles exceeded");
         const auto prev_profile = profile;
         ++profile;
         profile->set_boundary(*prev_profile);
diff --git a/include/ruckig/velocity.hpp b/include/ruckig/velocity.hpp
index adedc43..a7f19fd 100644
--- a/include/ruckig/velocity.hpp
+++ b/include/ruckig/velocity.hpp
@@ -20,8 +20,11 @@ class VelocityThirdOrderStep1 {
     double vd;
 
     // Max 3 valid profiles
-    using ProfileIter = std::array<Profile, 3>::iterator;
-    std::array<Profile, 3> valid_profiles;
+    static constexpr int max_num_valid_profiles = 3;
+    using Profiles =  std::array<Profile, max_num_valid_profiles + 1>;
+    using ProfileIter = Profiles::iterator;
+
+    Profiles valid_profiles;
 
     void time_acc0(ProfileIter& profile, double aMax, double aMin, double jMax, bool return_after_found) const;
     void time_none(ProfileIter& profile, double aMax, double aMin, double jMax, bool return_after_found) const;