#include "amr_control/amr_safety.h"

namespace amr_control
{
  AmrSafety::AmrSafety() {}

  AmrSafety::AmrSafety(const ros::NodeHandle &nh)
  {
    if (!initalized_)
      this->init(nh);
  }

  AmrSafety::~AmrSafety() {}

  void AmrSafety::init(const ros::NodeHandle &nh)
  {
    if (!initalized_)
    {

      initalized_ = true;
    }
  }

  bool AmrSafety::getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr)
  {
    if (plc_controller_ptr == nullptr)
      return false;
    else
    {
      plc_controller_ptr_ = plc_controller_ptr;
      return true;
    }
  }

  void AmrSafety::safetyHandle(
      nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel)
  {
    if (plc_controller_ptr_ == nullptr)
      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
    else if (!plc_controller_ptr_->checkConnected())
      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
    else
    {
      nav_2d_msgs::Twist2D velocity_warn;
      velocity_warn.x = 0.15;
      velocity_warn.theta = 0.5;
      nav_2d_msgs::Twist2D velocity_stop;

      // Dieu khien chuyen vung giam toc va mute
      bool zone_warn_cases;
      if (velocity.x >= velocity_warn.x)
        zone_warn_cases = false;
      else if (velocity.x < velocity_warn.x && velocity.x > 0.05)
        zone_warn_cases = true;
      plc_controller_ptr_->writeM(52, zone_warn_cases);

      // Nhan tin hieu 3 vung tu lidar
      bool output_lidar[4];
      plc_controller_ptr_->mulGetM(30, 33, output_lidar);
      if (output_lidar[0] && output_lidar[1] && output_lidar[2])
        cmd_vel = velocity_stop;
      else if (output_lidar[0] && !output_lidar[1] && output_lidar[2])
        cmd_vel = velocity_warn;
      else if (output_lidar[0] && !output_lidar[1] && !output_lidar[2])
        cmd_vel = cmd_vel_max;
      else
        cmd_vel = velocity_stop;
    }
  }

  void AmrSafety::writeMutesSafety(bool value)
  {
    if (plc_controller_ptr_ == nullptr)
      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
    else if (!plc_controller_ptr_->checkConnected())
      ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
    else
    {
      bool f_data;
      plc_controller_ptr_->getM(53, f_data);
      if (f_data != value)
        plc_controller_ptr_->writeM(53, value);
    }
  }
}