#ifndef __AMR_COTROLLER_H_INCLUDED_
#define __AMR_COTROLLER_H_INCLUDED_
#include <ros/ros.h>
#include <memory>
#include <string>
#include <condition_variable>
#include <pluginlib/class_loader.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <std_msgs/Bool.h>

#include "delta_modbus/delta_modbus_tcp.h"

// API
#include "amr_control/amr_opc_ua_server_api.h"
#include "amr_control/amr_vda_5050_client_api.h"

// Objects
#include <move_base_core/common.h>  // For TFListenerPtr
#include <move_base_core/navigation.h>
#include <loc_core/localization.h>
#include <amr_control/amr_monitor.h>
#include <nova5_control/imr_nova_control.h>
#include <amr_control/amr_safety.h>

// #include <robot/node_handle.h>
// Plugin Loader
// #include <robot/plugin_loader_helper.h>
#include <boost/dll/import.hpp>

namespace amr_control
{
  class AmrController
  {
  public:
    AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
    AmrController();
    virtual ~AmrController();

    void init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);

    /**
     * @brief Stop the ServerHandle
     */
    virtual void stop(void);

  protected:
    virtual void initalizingComunicationHandle(ros::NodeHandle &nh);
    virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh);
    virtual void initalizingLocalizationHandle(ros::NodeHandle &nh);
    virtual void initalizingMonitorHandle(ros::NodeHandle &nh);

  private:
    void ArmCallBack();

    void ArmDotuff();

    void unLoadCallBack();

    void conveyorBeltsShipping(amr_control::State &state);

    void loadCallBack();

    void conveyorBeltsReceiving(amr_control::State &state);

    void controllerDotuff();

    void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg);

    void threadHandle();

    bool initalized_;
    ros::NodeHandle nh_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
    // robot::TFListenerPtr tf_core_ptr_;

    ros::Subscriber is_detected_maker_sub_;

    std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
    std::shared_ptr<amr_control::VDA5050ClientAPI> vda_5050_client_api_ptr_;
    std::shared_ptr<std::thread> server_thr_;

    std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
    std::shared_ptr<std::thread> monitor_thr_;

    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
    boost::function<robot::move_base_core::BaseNavigation::Ptr()> move_base_loader_;

    // Synchronous processing thread
    std::shared_ptr<std::thread> move_base_thr_;
    std::mutex init_move_base_mutex_;
    std::condition_variable init_move_base_cv_;
    bool move_base_initialized_;

    // Safety Speed
    std::shared_ptr<AmrSafety> amr_safety_ptr_;
    bool muted_;
    std::mutex cmd_vel_mtx;
    nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_recommended_;

    std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
    pluginlib::ClassLoader<loc_core::BaseLocalization> loc_base_loader_;
    // Synchronous processing thread
    std::shared_ptr<std::thread> loc_base_thr_;
    std::mutex init_loc_base_mutex_;
    std::condition_variable init_loc_base_cv_;
    bool loc_base_initialized_;

    bool cancel_, enable_ ,pause_;
    // Belt properties
    std::thread belt_thread_;
    amr_control::State cur_belt_state_en_;
    bool belt_joined_;
    std::mutex belt_mutex_;

    // amr propertise
    std::thread arm_thread_; 
    bool arm_joined_;
    std::mutex arm_mutex_;
    bool arm_continue_;
    bool arm_go_home_;
    bool arm_power_on_;
    unsigned int count_ng_max_, count_ok_max_;
    unsigned int *status_code_ptr_ = new unsigned int;

  }; // class AMRController
} // namespace amr_control

#endif // __AMR_COTROLLER_H_INCLUDED_