#ifndef __AMR_SAFETY_H_INCLUDED_
#define __AMR_SAFETY_H_INCLUDED_
#include <ros/ros.h>
#include <memory>
#include <string>
#include <nav_2d_msgs/Twist2D.h>
#include "delta_modbus/delta_modbus_tcp.h"

namespace amr_control
{
  class AmrSafety
  {
  public:
    /**
     * @brief Default constructor for AmrSafety
     */
    AmrSafety();

    /**
     * @brief Constructor for AmrSafety with ROS node handle
     * @param nh ROS NodeHandle for initialization
     */
    AmrSafety(const ros::NodeHandle &nh);

    /**
     * @brief Virtual destructor for AmrSafety
     */
    virtual ~AmrSafety();

    /**
     * @brief Initialize the ServerHandle with a ROS node handle
     * @param nh ROS NodeHandle for initialization
     */
    virtual void init(const ros::NodeHandle &nh);

    /**
     * @brief Get object PLC controller
     * @param plc_controller_ptr A smart pointer of the PLC object
     */
    virtual bool getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr);

    /**
     * @brief Safety Hanlde
     */
    virtual void safetyHandle(nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel);

    virtual void writeMutesSafety(bool value);

  private:
    bool initalized_;
    std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
  };
} // namespace amr_control

#endif // __AMR_SAFETY_H_INCLUDED_