#ifndef _SCORE_ALGORITHM_H_INCLUDED__
#define _SCORE_ALGORITHM_H_INCLUDED__

#ifdef BUILD_WITH_ROS
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#endif
#include <robot/robot.h>
#include <iostream>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>

#include <robot_nav_core2/common.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>

#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <mkt_msgs/Trajectory2D.h>
#include <score_algorithm/trajectory_generator.h>

#define FORWARD 1
#define BACKWARD -1

namespace score_algorithm
{
  class ScoreAlgorithm
  {
  public:
    using Ptr = std::shared_ptr<score_algorithm::ScoreAlgorithm>;

    inline double smoothstep(double x)
    {
      x = std::clamp(x, 0.0, 1.0);
      return x * x * (3 - 2 * x);
    }

    /**
     * @brief Initialize parameters as needed
     * @param name Namespace for this planner
     * @param tf TFListener pointer
     * @param costmap_robot Costmap pointer
     */
    virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                            const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
  
    /**
     * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
     *
     * Subclasses may overwrite. Return false in case there is any error.
     *
     * @param pose Current pose (costmap frame)
     * @param velocity Current velocity
     * @param goal The final goal (costmap frame)
     * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
     */
    virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
                         const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                         double &x_direction, double &y_direction, double &theta_direction) = 0;

    /**
     * @brief Calculating algorithm
     * @param pose
     * @param velocity
     * @param traj
     */
    virtual mkt_msgs::Trajectory2D calculator(
        const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0;

    /**
     * @brief Reset all data
     */
    virtual void reset() {};

    /**
     * @brief Stoping move navigation
     */
    virtual void stop() {};

    /**
     * @brief resume move navigation after stopped
     */
    virtual void resume() {};

    std::string getName()
    {
      return name_;
    }

  protected:
    double sign(double x)
    {
      return x < 0.0 ? -1.0 : 1.0;
    }

    /**
     * @brief Get goal index
     * @param robot_pose
     * @param plan
     * @param distance
     * @param start_index
     * @param last_valid_index
     * @return goal index
     */
    virtual unsigned int getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan,
                                      const double distance, unsigned int start_index, unsigned int last_valid_index) const;

    /**
     * @brief Get goal pose
     * @param robot_pose
     * @param velocity
     * @param global_plan
     * @param sub_goal
     * @param S
     * @return true if goal pose is found, false otherwise
     */
    virtual bool computePlanCommand(const robot_nav_2d_msgs::Pose2DStamped &robot_pose, const robot_nav_2d_msgs::Twist2D &velocity, const double &S,
                                    const robot_nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, robot_nav_2d_msgs::Path2D &result);

    /**
     * @brief Find sub goal index
     * @param plan
     * @param index_s
     * @return true if sub goal index is found, false otherwise
     */
    virtual bool findSubGoalIndex(const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<robot_nav_2d_msgs::Pose2DStamped> &mutes);

    /**
     * @brief Calculate journey
     * @param plan
     * @param start_index
     * @param last_valid_index
     * @return journey
     */
    double journey(const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;

    /**
     * @brief  Check whether the robot is stopped or not
     * @param velocity The current odometry information for the robot
     * @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
     * @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
     * @return True if the robot is stopped, false otherwise
     */
    bool stopped(const robot_nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity);

    void clear();

    std::string name_;
    TFListenerPtr tf_;
    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
    TrajectoryGenerator::Ptr traj_;

    int index_samples_;
    bool follow_step_path_;
    double xy_local_goal_tolerance_;
    double old_xy_goal_tolerance_;
    double angle_threshold_;

#ifdef BUILD_WITH_ROS
    ros::Publisher current_goal_pub_;
    ros::Publisher closet_robot_goal_pub_;
#endif

    std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
    std::vector<unsigned int> start_index_saved_vt_;
    unsigned int sub_goal_index_saved_, sub_goal_seq_saved_;
    unsigned int sub_start_index_saved_, sub_start_seq_saved_;

    double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
    double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
    double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
    double min_speed_xy_, max_speed_xy_;
    double rot_stopped_velocity_, trans_stopped_velocity_;
  };
} // namespace score_algorithm

#endif // SCORE_ALGORITHM_H_INCLUDED__