#ifndef MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_
#define MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_

#include <mkt_plugins/velocity_iterator.h>
#include <mkt_plugins/one_d_velocity_iterator.h>
#include <memory>

namespace mkt_plugins
{
  /**
   * @class XYThetaIterator
   * @brief Iterator that generates velocity samples for X, Y, and Theta dimensions
   * 
   * This iterator combines three OneDVelocityIterator instances (one for each dimension)
   * to generate valid velocity combinations that satisfy kinematic constraints.
   */
  class XYThetaIterator : public VelocityIterator
  {
  public:
    /**
     * @brief Default constructor
     * Initializes all iterators to nullptr
     */
    XYThetaIterator();

    /**
     * @brief Destructor
     */
    virtual ~XYThetaIterator();
    
    /**
     * @brief Initialize the iterator with parameters and kinematics
     * @param nh NodeHandle for loading sampling parameters (vx_samples, vy_samples, vtheta_samples)
     * @param kinematics Shared pointer to kinematic parameters for validation
     */
    void initialize(const robot::NodeHandle& nh, KinematicParameters::Ptr kinematics) override;
    
    /**
     * @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
     * 
     * Creates new OneDVelocityIterator instances for X, Y, and Theta dimensions
     * based on the current velocity and kinematic constraints.
     */
    void startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) override;
    
    /**
     * @brief Check if there are more velocity samples to iterate
     * @return True if there are more valid velocity combinations available
     */
    bool hasMoreTwists() override;
    
    /**
     * @brief Get the next velocity sample
     * @return Next valid velocity combination (x, y, theta)
     * 
     * Returns the next valid velocity that satisfies kinematic constraints.
     * Automatically iterates to the next valid velocity if current is invalid.
     */
    robot_nav_2d_msgs::Twist2D nextTwist() override;

    /**
     * @brief Get the current velocity
     * @return The current velocity (x, y, theta)
     */
    robot_nav_2d_msgs::Twist2D getTwist() override;
    
  protected:
    /**
     * @brief Check if the current velocity combination is valid
     * @return True if the current velocity (x_it_, y_it_, th_it_) satisfies kinematic constraints
     */
    virtual bool isValidVelocity();
    
    /**
     * @brief Iterate through velocity samples until a valid combination is found
     * 
     * Increments the iterators (theta first, then y, then x) until a valid
     * velocity combination is found or all combinations are exhausted.
     */
    void iterateToValidVelocity();

    int vx_samples_, vy_samples_, vtheta_samples_;
    KinematicParameters::Ptr kinematics_;
    std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
  };
}  // namespace mkt_plugins

#endif  // MKT_PLUGINS_XY_THETA_ITERATOR_H_INCLUDED_
