#ifndef MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_
#define MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_

#include <robot/robot.h>
#include <score_algorithm/trajectory_generator.h>
#include <mkt_plugins/velocity_iterator.h>
#include <mkt_plugins/kinematic_parameters.h>
#include <vector>
#include <memory>

namespace mkt_plugins
{
  /**
   * @class StandardTrajectoryGenerator
   * @brief Standard MKT-like trajectory generator.
   */
  class StandardTrajectoryGenerator : public score_algorithm::TrajectoryGenerator
  {
  public:

    /**
     * @brief Constructor
     */
    StandardTrajectoryGenerator();

    /**
     * @brief Destructor
     */
    virtual ~StandardTrajectoryGenerator();
    /**
     * @brief Initialize the trajectory generator with parameters from NodeHandle
     * @param nh NodeHandle for loading configuration parameters
     * 
     * Loads parameters such as sim_time, discretize_by_time, time_granularity,
     * linear_granularity, angular_granularity, and include_last_point.
     */
    void initialize(const robot::NodeHandle &nh) override;

    /**
     * @brief Set the direction array for X, Y, and Theta movement
     * @param xytheta_direct Pointer to array of 3 integers representing direction
     *                       (1 for forward/right/clockwise, -1 for backward/left/counterclockwise)
     */
    void setDirect(int *xytheta_direct) override;

    /**
     * @brief set velocity for x, y asix of the robot
     * @param linear the velocity command
     * @return True if set is done, false otherwise
     */
    bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;

    /**
     * @brief get velocity for x, y asix of the robot
     * @param linear The velocity command
     * @return True if set is done, false otherwise
     */
    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;

    /**
     * @brief Set velocity theta for z asix of the robot
     * @param angular the command velocity
     * @return True if set is done, false otherwise
     */
    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;

    /**
     * @brief Get velocity theta for z asix of the robot
     * @param direct The velocity command
     * @return True if set is done, false otherwise
     */
    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;

    /**
     * @brief Start a new iteration with current velocity
     * @param current_velocity Current velocity of the robot
     * 
     * Resets the velocity iterator and prepares it to generate velocities
     * reachable from the current velocity.
     */
    void startNewIteration(const robot_nav_2d_msgs::Twist2D &current_velocity) override;
    
    /**
     * @brief Check if there are more velocity samples available
     * @return True if there are more valid velocity combinations to iterate
     */
    bool hasMoreTwists() override;
    
    /**
     * @brief Get the next velocity sample
     * @return Next valid velocity combination
     */
    robot_nav_2d_msgs::Twist2D nextTwist() override;

    /**
     * @brief Get the current velocity
     * @return The current velocity
     */
    robot_nav_2d_msgs::Twist2D getTwist() override;
    
    /**
     * @brief Generate a trajectory from start pose to goal
     * @param start_pose Starting pose of the robot
     * @param transformed_plan Transformed global plan (path to follow)
     * @param start_vel Starting velocity of the robot
     * @param cmd_vel Command velocity to achieve
     * @return Generated trajectory containing poses and velocities over time
     * 
     * Generates a trajectory by:
     * 1. Computing velocity steps based on acceleration limits
     * 2. Projecting poses forward using kinematic model
     * 3. Sampling velocities using the velocity iterator
     */
    mkt_msgs::Trajectory2D generateTrajectory(const robot_nav_2d_msgs::Pose2DStamped &start_pose,
                                              const robot_nav_2d_msgs::Path2D &transformed_plan,
                                              const robot_nav_2d_msgs::Twist2D &start_vel,
                                              const robot_nav_2d_msgs::Twist2D &cmd_vel) override;

    /**
     * @brief Get the NodeHandle used for kinematics configuration
     * @return NodeHandle scoped to kinematics namespace
     */
    virtual robot::NodeHandle getNodeHandle() override;

    /**
     * @brief Factory function to create a StandardTrajectoryGenerator instance
     * @return Shared pointer to a new StandardTrajectoryGenerator instance
     */
    static score_algorithm::TrajectoryGenerator::Ptr create();

  protected:
    /**
     * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
     */
    virtual void initializeIterator(const robot::NodeHandle &nh);

    /**
     * @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
     *
     * @param cmd_vel Desired velocity
     * @param start_vel starting velocity
     * @param dt amount of time in seconds
     * @return new velocity after dt seconds
     */
    virtual robot_nav_2d_msgs::Twist2D computeNewVelocity(const robot_nav_2d_msgs::Twist2D &cmd_vel,
                                                    const robot_nav_2d_msgs::Twist2D &start_vel,
                                                    const double dt);

    /**
     * @brief Use the robot's kinematic model to predict new positions for the robot
     *
     * @param start_pose Starting pose
     * @param vel Actual robot velocity (assumed to be within acceleration limits)
     * @param dt amount of time in seconds
     * @return New pose after dt seconds
     */
    virtual robot_geometry_msgs::Pose2D computeNewPosition(const robot_geometry_msgs::Pose2D start_pose,
                                                     const robot_nav_2d_msgs::Twist2D &vel,
                                                     const double dt);

    /**
     * @brief Compute an array of time deltas between the points in the generated trajectory.
     *
     * @param cmd_vel The desired command velocity
     * @return vector of the difference between each time step in the generated trajectory
     *
     * If we are discretizing by time, the returned vector will be the same constant time_granularity
     * for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
     *
     * Right now the vector contains a single value repeated many times, but this method could be overridden
     * to allow for dynamic spacing
     */
    virtual std::vector<double> getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel);

    robot::NodeHandle nh_kinematics_;
    KinematicParameters::Ptr kinematics_;
    std::shared_ptr<mkt_plugins::VelocityIterator> velocity_iterator_;

    double sim_time_;

    // Sampling Parameters
    bool discretize_by_time_;
    double time_granularity_;    ///< If discretizing by time, the amount of time between each point in the traj
    double linear_granularity_;  ///< If not discretizing by time, the amount of linear space between points
    double angular_granularity_; ///< If not discretizing by time, the amount of angular space between points

    /* Backwards Compatibility Parameter: include_last_point
     *
     * dwa had an off-by-one error built into it.
     * It generated N trajectory points, where N = ceil(sim_time / time_delta).
     * If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which
     * indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the
     * actual sim_time was much less than advertised.
     *
     * This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true.
     *
     * Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
     * were not projected out as far as they intended.
     */
    bool include_last_point_;
  }; // class StandardTrajectoryGenerator

} // namespace dwb_plugins

#endif // MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_