#ifndef MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_
#define MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_

#include <robot/robot.h>
#include <memory>

namespace mkt_plugins
{

  /**
   * @class KinematicParameters
   * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
   */
  class KinematicParameters
  {
  public:
    KinematicParameters();
    virtual ~KinematicParameters();
    void initialize(const robot::NodeHandle &nh);

    /**
     * @brief Get the direction array for X, Y, and theta movement
     * @return Pointer to an array of integers representing the direction of movement for X, Y, and theta (e.g., 1 for forward/right/clockwise, -1 for backward/left/counterclockwise, 0 for stop)
     *
     * This array controls the intended direction of the robot. Sudden changes in the theta component can cause discontinuities in odometry data, potentially leading to localization jumps in AMCL when rotating in place.
     */
    int *getDirect();

    /**
     * @brief Get the minimum linear velocity along the X-axis
     * @return Minimum linear velocity (m/s) for forward/backward motion
     *
     * Used to ensure the robot does not move too slowly along X, which could lead to odometry drift. Less relevant for in-place rotation issues.
     */
    double getMinX();

    /**
     * @brief Get the maximum linear velocity along the X-axis
     * @return Maximum linear velocity (m/s) for forward/backward motion
     *
     * Ensures the robot does not exceed safe speeds along X. Not directly related to rotation-induced localization jumps.
     */
    double getMaxX();

    /**
     * @brief Get the maximum linear acceleration along the X-axis
     * @return Maximum linear acceleration (m/s²) for forward/backward motion
     *
     * High acceleration can cause odometry errors if the robot's hardware cannot keep up. Less relevant for in-place rotation.
     */
    double getAccX();

    /**
     * @brief Get the maximum linear deceleration along the X-axis
     * @return Maximum linear deceleration (m/s²) for forward/backward motion
     *
     * Improper deceleration may cause jerky stops, leading to minor odometry errors. Not a primary cause of rotation-induced localization jumps.
     */
    double getDecelX();

    /**
     * @brief Get the minimum linear velocity along the Y-axis
     * @return Minimum linear velocity (m/s) for lateral motion (if applicable)
     *
     * Typically zero for differential drive robots. Not relevant for in-place rotation.
     */
    double getMinY();

    /**
     * @brief Get the maximum linear velocity along the Y-axis
     * @return Maximum linear velocity (m/s) for lateral motion
     *
     * Typically zero for differential drive robots. Not relevant for in-place rotation.
     */
    double getMaxY();

    /**
     * @brief Get the maximum linear acceleration along the Y-axis
     * @return Maximum linear acceleration (m/s²) for lateral motion
     *
     * Not relevant for in-place rotation in differential drive robots.
     */
    double getAccY();

    /**
     * @brief Get the maximum linear deceleration along the Y-axis
     * @return Maximum linear deceleration (m/s²) for lateral motion
     *
     * Not relevant for in-place rotation in differential drive robots.
     */
    double getDecelY();

    /**
     * @brief Get the minimum combined linear velocity in the XY plane
     * @return Minimum combined velocity (m/s) for motion in the XY plane
     *
     * Ensures the robot moves at a meaningful speed in the XY plane. Not directly related to in-place rotation issues.
     */
    double getMinSpeedXY();

    /**
     * @brief Get the minimum angular velocity for rotation
     * @return Minimum angular velocity (rad/s) for rotation around the Z-axis
     *
     * A very low value may cause the robot to struggle with stable rotation, leading to odometry errors and potential localization jumps in AMCL.
     */
    double getMinTheta();

    /**
     * @brief Get the maximum angular velocity for rotation
     * @return Maximum angular velocity (rad/s) for rotation around the Z-axis
     *
     * A high value (e.g., >2.0 rad/s) can cause wheel slip or odometry errors during fast rotation, leading to localization jumps in AMCL.
     */
    double getMaxTheta();

    /**
     * @brief Get the maximum angular acceleration for rotation
     * @return Maximum angular acceleration (rad/s²) for rotation around the Z-axis
     *
     * High angular acceleration can cause jerky rotation, leading to odometry errors and potential localization jumps in AMCL.
     */
    double getAccTheta();

    /**
     * @brief Get the maximum angular deceleration for rotation
     * @return Maximum angular deceleration (rad/s²) for rotation around the Z-axis
     *
     * Improper deceleration can cause jerky stops during rotation, leading to odometry errors and localization jumps in AMCL.
     */
    double getDecelTheta();

    /**
     * @brief Get the minimum angular speed for rotation
     * @return Minimum angular speed (rad/s) for rotation around the Z-axis
     *
     * Similar to getMinTheta(), a very low value may cause unstable rotation, contributing to odometry errors and AMCL localization issues.
     */
    double getMinSpeedTheta();

    /**
     * @brief Set the direction array for X, Y, and theta movement
     * @param xytheta_direct Pointer to an array of integers specifying the direction for X, Y, and theta
     *
     * Sets the intended direction of motion. Sudden changes in the theta component can cause discontinuities in odometry, leading to localization jumps in AMCL during in-place rotation.
     */
    void setDirect(int *xytheta_direct);

    /**
     * @brief Set the minimum linear velocity along the X-axis
     * @param value Minimum linear velocity (m/s) for forward/backward motion
     *
     * Ensures the robot does not move too slowly along X. Not directly related to in-place rotation issues.
     */
    void setMinX(double value);

    /**
     * @brief Set the maximum linear velocity along the X-axis
     * @param value Maximum linear velocity (m/s) for forward/backward motion
     *
     * Limits the robot's speed along X. Not directly related to in-place rotation issues.
     */
    void setMaxX(double value);

    /**
     * @brief Set the maximum linear acceleration along the X-axis
     * @param value Maximum linear acceleration (m/s²) for forward/backward motion
     *
     * High acceleration can cause odometry errors if not supported by hardware. Less relevant for in-place rotation.
     */
    void setAccX(double value);

    /**
     * @brief Set the maximum linear deceleration along the X-axis
     * @param value Maximum linear deceleration (m/s²) for forward/backward motion
     *
     * Improper deceleration may cause minor odometry errors. Not a primary cause of rotation-induced localization jumps.
     */
    void setDecelX(double value);

    /**
     * @brief Set the minimum linear velocity along the Y-axis
     * @param value Minimum linear velocity (m/s) for lateral motion
     *
     * Typically zero for differential drive robots. Not relevant for in-place rotation.
     */
    void setMinY(double value);

    /**
     * @brief Set the maximum linear velocity along the Y-axis
     * @param value Maximum linear velocity (m/s) for lateral motion
     *
     * Typically zero for differential drive robots. Not relevant for in-place rotation.
     */
    void setMaxY(double value);

    /**
     * @brief Set the maximum linear acceleration along the Y-axis
     * @param value Maximum linear acceleration (m/s²) for lateral motion
     *
     * Not relevant for in-place rotation in differential drive robots.
     */
    void setAccY(double value);

    /**
     * @brief Set the maximum linear deceleration along the Y-axis
     * @param value Maximum linear deceleration (m/s²) for lateral motion
     *
     * Not relevant for in-place rotation in differential drive robots.
     */
    void setDecelY(double value);

    /**
     * @brief Set the minimum combined linear velocity in the XY plane
     * @param value Minimum combined velocity (m/s) for motion in the XY plane
     *
     * Ensures meaningful motion in the XY plane. Not directly related to in-place rotation issues.
     */
    void setMinSpeedXY(double value);

    /**
     * @brief Set the maximum angular velocity for rotation
     * @param value Maximum angular velocity (rad/s) for rotation around the Z-axis
     *
     * A high value can cause fast rotation, leading to wheel slip or odometry errors, which may cause AMCL to jump during in-place rotation.
     */
    void setMaxTheta(double value);

    /**
     * @brief Set the maximum angular acceleration for rotation
     * @param value Maximum angular acceleration (rad/s²) for rotation around the Z-axis
     *
     * High acceleration can cause jerky rotation, leading to odometry errors and AMCL localization jumps.
     */
    void setAccTheta(double value);

    /**
     * @brief Set the maximum angular deceleration for rotation
     * @param value Maximum angular deceleration (rad/s²) for rotation around the Z-axis
     *
     * Improper deceleration can cause jerky stops, leading to odometry errors and AMCL localization jumps.
     */
    void setDecelTheta(double value);

    /**
     * @brief Set the minimum angular speed for rotation
     * @param value Minimum angular speed (rad/s) for rotation around the Z-axis
     *
     * A very low value may cause unstable rotation, contributing to odometry errors and AMCL localization issues.
     */
    void setMinSpeedTheta(double value);

    /**
     * @brief Check to see whether the combined x/y/theta velocities are valid
     * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits
     *
     * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta.
     * The speed is valid if
     *  1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative)
     *  AND
     *  2) min_speed_xy is negative or min_speed_theta is negative or
     *     hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta.
     *
     * In English, it makes sure the diagonal motion is not too fast,
     * and that the velocity is moving in some meaningful direction.
     *
     * In Latin, quod si motus sit signum quaerit et movere ieiunium et significantissime comprehendite.
     */
    bool isValidSpeed(double x, double y, double theta);

    using Ptr = std::shared_ptr<KinematicParameters>;

  protected:
    
    /**
     * @brief Configure the kinematic parameters
     * @param nh NodeHandle
     *
     * Configures the kinematic parameters from the given NodeHandle.
     */
    void configure(const robot::NodeHandle &nh);

    /**
     * @brief Reconfigure the kinematic parameters
     * @param nh NodeHandle
     *
     * Reconfigures the kinematic parameters from the given NodeHandle.
     */
    void reconfigure(const robot::NodeHandle &nh);
  
    // For parameter descriptions, see cfg/KinematicParams.cfg
    int xytheta_direct_[3];
    double min_vel_x_, min_vel_y_;
    double max_vel_x_, max_vel_y_, max_vel_theta_;

    double min_speed_xy_, max_speed_xy_;
    double min_speed_theta_;

    double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
    double decel_lim_x_, decel_lim_y_, decel_lim_theta_;

    // Cached square values of min_speed_xy and max_speed_xy
    double min_speed_xy_sq_, max_speed_xy_sq_;

  private:
    double original_min_vel_x_, original_min_vel_y_;
    double original_max_vel_x_, original_max_vel_y_, original_max_vel_theta_;

    double original_min_speed_xy_, original_max_speed_xy_;
    double original_min_speed_theta_;

    double original_acc_lim_x_, original_acc_lim_y_, original_acc_lim_theta_;
    double original_decel_lim_x_, original_decel_lim_y_, original_decel_lim_theta_;
  };

} // namespace mkt_plugins

#endif // MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_
