#ifndef __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_
#define __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_

#include <ros/ros.h>
#include "amr_comunication/vda_5050/vda_5050_connector.h"
#include "move_base_core/navigation.h"
#include "loc_core/localization.h"
#include "amr_control/amr_monitor.h"
#include "amr_control/common.h"
#include <nav_2d_utils/odom_subscriber.h>
#include <nav_2d_msgs/Twist2D.h>

namespace amr_control
{
  class VDA5050ClientAPI
  {
  public:
    /**
     * @brief Contructor
     */
    VDA5050ClientAPI();

    /**
     * @brief Contructor
     * @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện.
     * @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot.
     * @param loc_base Con trỏ đến đối tượng LocBase.
     */
    VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<robot::move_base_core::BaseNavigation> move_base,
                     std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<amr_control::AmrMonitor> monitor);

    /**
     * @brief Detructor
     */
    virtual ~VDA5050ClientAPI();

  protected:
    virtual void orderWorker();

    virtual void moveTo(vda_5050::Order order, uint8_t &status, std::string &message);

    virtual void moveToDock(vda_5050::Order order, uint8_t &status, std::string &message);

    virtual void rotateTo(vda_5050::Order order, uint8_t &status, std::string &message);

    virtual void instantActionWorker();

    virtual void excuteAction();

    virtual void updateVelocity(double velocity);

    virtual void updateAngular(double angular);

    virtual void cancelOrderCallBack(std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void cancelOrder(std::shared_ptr<vda_5050::ActionState> action_state);

    template <typename T>
    void excuteNoneAction(
        void (T::*excute)(std::shared_ptr<vda_5050::Action>, std::shared_ptr<vda_5050::ActionState>), T *obj,
        std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void mutedSafetyON(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    virtual void mutedSafetyOFF(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);

    void updating();

    void updatingvda5050Visualization();

    void updatingvda5050State();

    void resetState();

    // properties
    std::shared_ptr<amr_comunication::VDA5050Connector> client_ptr_;
    static std::shared_ptr<robot::move_base_core::BaseNavigation> move_base_ptr_;
    static std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
    static std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;

    ros::Publisher plan_pub_;
    std::string global_plan_msg_type_;
    std::thread update_thread_;

    std::map<std::string, std::thread> threads_map_;
    std::mutex threads_mutex;

  public:
    // Belt Convey
    amr_control::State *cur_belt_state_en_;
    std::function<void()> unLoad_excuted_;
    std::function<void()> load_excuted_;
    std::thread *belt_thread_ptr_;
    std::mutex belt_mutex_;
    bool belt_joined_;

    // arm control
    std::thread *arm_thread_ptr_;
    std::function<void()> arm_function_ptr_;
    unsigned int *count_ng_max_, *count_ok_max_;

    bool *muted_value_;
    bool *pause_action_;
    bool *cancel_action_;
    bool *enable_action_;
    vda_5050::State::OperatingMode mode_;
    nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_max_saved_;
  };

} // namespace amr_control

#endif // __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_