#ifndef MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
#define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_

#include <robot/robot.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <mkt_plugins/kinematic_parameters.h>

namespace mkt_plugins
{
/**
 * @class VelocityIterator
 * @brief Abstract base class for iterating over valid velocity samples
 * 
 * This interface provides a way to generate and iterate through velocity samples
 * that satisfy kinematic constraints. Implementations should generate velocities
 * reachable from the current velocity within acceleration limits.
 */
class VelocityIterator
{
public:
  /**
   * @brief Virtual destructor
   */
  virtual ~VelocityIterator() {}
  
  /**
   * @brief Initialize the iterator with parameters and kinematics
   * @param nh NodeHandle for loading configuration parameters
   * @param kinematics Shared pointer to kinematic parameters for validation
   */
  virtual void initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) = 0;
  
  /**
   * @brief Start a new iteration with current velocity and time delta
   * @param current_velocity Current velocity of the robot
   * @param dt Time delta for velocity projection
   * 
   * Resets the iterator and prepares it to generate velocities reachable
   * from the current velocity within dt seconds.
   */
  virtual void startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) = 0;
  
  /**
   * @brief Check if there are more velocity samples available
   * @return True if there are more valid velocity combinations to iterate
   */
  virtual bool hasMoreTwists() = 0;
  
  /**
   * @brief Get the next velocity sample
   * @return Next valid velocity combination
   * 
   * Returns the next velocity sample that satisfies kinematic constraints.
   * Should only be called when hasMoreTwists() returns true.
   */
  virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;

  /**
   * @brief Get the current velocity
   * @return The current velocity
   */
  virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
};  // class VelocityIterator

}  // namespace mkt_plugins

#endif  // MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
