#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 <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.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 <amr_control/tf_converter.h>
#include <amr_control/sensor_converter.h>
#include <amr_control/amr_publiser.h>
#include <amr_control/amr_subcriber.h>

#include <boost/dll/import.hpp>

namespace amr_control
{
  class AmrController
  {
  public:
    /**
     * @brief Constructor with NodeHandle and TF buffer
     * @param nh ROS NodeHandle for communication
     * @param buffer Shared pointer to tf2_ros::Buffer for coordinate transforms
     */
    AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
    
    /**
     * @brief Default constructor
     */
    AmrController();
    
    /**
     * @brief Destructor - cleans up threads and resources
     */
    virtual ~AmrController();

    /**
     * @brief Initialize the AMR controller with NodeHandle and TF buffer
     * @param nh ROS NodeHandle for communication
     * @param buffer Shared pointer to tf2_ros::Buffer for coordinate transforms
     */
    void init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);

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

  protected:
    /**
     * @brief Initialize communication handles (OPC UA server and VDA5050 client)
     * @param nh ROS NodeHandle for communication setup
     */
    virtual void initalizingComunicationHandle(ros::NodeHandle &nh);
    
    /**
     * @brief Initialize move_base navigation system and subscribe to sensor topics
     * @param nh ROS NodeHandle for move_base configuration
     */
    virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh);
    
    /**
     * @brief Initialize localization system (loc_base)
     * @param nh ROS NodeHandle for localization configuration
     */
    virtual void initalizingLocalizationHandle(ros::NodeHandle &nh);
    
    /**
     * @brief Initialize monitoring system (AmrMonitor)
     * @param nh ROS NodeHandle for monitor configuration
     */
    virtual void initalizingMonitorHandle(ros::NodeHandle &nh);

  private:  

    /**
     * @brief Callback for arm control - starts arm thread for robot arm operations
     */
    void ArmCallBack();

    /**
     * @brief Callback for load operation - starts receiving conveyor belt thread
     */
    void loadCallBack();

    /**
     * @brief Callback for unload operation - starts shipping conveyor belt thread
     */
    void unLoadCallBack();

    /**
     * @brief Callback for marker detection status
     * @param msg Boolean message indicating if marker is detected
     */
    void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg);

    /**
     * @brief Control shipping conveyor belt via PLC communication
     * @param state Reference to state variable that tracks belt operation status
     */
    void conveyorBeltsShipping(amr_control::State &state);

    /**
     * @brief Control receiving conveyor belt via PLC communication
     * @param state Reference to state variable that tracks belt operation status
     */
    void conveyorBeltsReceiving(amr_control::State &state);

    /**
     * @brief Main controller loop - handles PLC communication, safety checks, and velocity control
     */
    void controllerDotuff();

    /**
     * @brief Arm control thread function - manages robot arm operations (home, mode control)
     */
    void ArmDotuff();

    /**
     * @brief Main thread handler - coordinates initialization and main control loop
     */
    void threadHandle();

    bool initalized_;
    ros::NodeHandle nh_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
    robot::TFListenerPtr tf_core_ptr_;
    std::shared_ptr<amr_control::TfConverter> tf_converter_ptr_;
    std::shared_ptr<amr_control::SensorConverter> sensor_converter_ptr_;
    std::shared_ptr<amr_control::AmrPublisher> amr_publisher_ptr_;
    std::shared_ptr<amr_control::AmrSubscriber> amr_subscriber_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_