#ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
#define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H

#include <robot/robot.h>
#include <mkt_msgs/Trajectory2D.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <memory>
#include <vector>
#include <robot_geometry_msgs/Vector3.h>

namespace score_algorithm
{

  /**
   * @class TrajectoryGenerator
   * @brief Interface for iterating through possible velocities and creating trajectories
   *
   * This class defines the plugin interface for two separate but related components.
   *
   * First, this class provides an iterator interface for exploring all of the velocities
   * to search, given the current velocity.
   *
   * Second, the class gives an independent interface for creating a trajectory from a twist,
   * i.e. projecting it out in time and space.
   *
   * Both components rely heavily on the robot's kinematic model, and can share many parameters,
   * which is why they are grouped into a singular class.
   */
  class TrajectoryGenerator
  {
  public:
    using Ptr = std::shared_ptr<score_algorithm::TrajectoryGenerator>;

    virtual ~TrajectoryGenerator() {}

    /**
     * @brief Initialize parameters as needed
     * @param nh NodeHandle to read parameters from
     */
    virtual void initialize(const robot::NodeHandle &nh) = 0;

    /**
     * @brief Reset the state (if any) when the planner gets a new goal
     */
    virtual void reset() {}

    /**
     * @brief Start a new iteration based on the current velocity
     * @param current_velocity
     */
    virtual void startNewIteration(const robot_nav_2d_msgs::Twist2D &current_velocity) = 0;

    /**
     * @brief Set direct of robot based on the current velocity
     * @param direct
     */
    virtual void setDirect(int *xytheta_direct) = 0;

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

    /**
     * @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) = 0;

    /**
     * @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) = 0;

    /**
     * @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) = 0;

    /**
     * @brief Test to see whether there are more twists to test
     * @return True if more twists, false otherwise
     */
    virtual bool hasMoreTwists() = 0;

    /**
     * @brief Return the next twist and advance the iteration
     * @return The Twist!
     */
    virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;

    /**
     * @brief Get the current twist
     * @return The current twist
     */
    virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;

    /**
     * @brief Get all the twists for an iteration.
     *
     * Note: Resets the iterator if one is in process
     *
     * @param current_velocity
     * @return all the twists
     */
    virtual std::vector<robot_nav_2d_msgs::Twist2D> getTwists(const robot_nav_2d_msgs::Twist2D &current_velocity)
    {
      std::vector<robot_nav_2d_msgs::Twist2D> twists;
      startNewIteration(current_velocity);
      while (hasMoreTwists())
      {
        twists.push_back(nextTwist());
      }
      return twists;
    }

    /**
     * @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
     * @param start_pose Current robot location
     * @param start_vel Current robot velocity
     * @param cmd_vel The desired command velocity
     */
    virtual 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) = 0;

    virtual robot::NodeHandle getNodeHandle() = 0;

  };

} // namespace score_algorithm

#endif // _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
