#include "amr_control/amr_vda_5050_client_api.h"
#include "amr_control/amr_make_plan_with_order.h"
#include <functional>
#include <optional>
#include "vda5050_msgs/Order.h"
#include "robot_nav_msgs/Path.h"
#include "robot_geometry_msgs/Pose2D.h"
#include "robot_nav_2d_utils/conversions.h"

namespace amr_control
{
  VDA5050ClientAPI::VDA5050ClientAPI()
  {
    this->client_ptr_ = std::make_shared<amr_comunication::VDA5050Connector>();
    this->client_ptr_->execute_order_ = std::bind(&VDA5050ClientAPI::orderWorker, this);
    this->client_ptr_->execute_instant_action_ = std::bind(&VDA5050ClientAPI::instantActionWorker, this);

    this->cancel_action_ = new bool;
    this->pause_action_ = new bool;
    this->enable_action_ = new bool;
    this->muted_value_ = new bool;
    this->count_ng_max_ = new unsigned int(1);
    this->count_ok_max_ = new unsigned int(1);
  }

  VDA5050ClientAPI::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<AmrMonitor> monitor)
  {
    VDA5050ClientAPI::move_base_ptr_ = move_base;
    VDA5050ClientAPI::loc_base_ptr_ = loc_base;
    VDA5050ClientAPI::monitor_ptr_ = monitor;

    this->cancel_action_ = new bool;
    this->pause_action_ = new bool;
    this->enable_action_ = new bool;
    this->muted_value_ = new bool;

    this->client_ptr_ = std::make_shared<amr_comunication::VDA5050Connector>();
    this->client_ptr_->execute_order_ = std::bind(&VDA5050ClientAPI::orderWorker, this);
    this->client_ptr_->execute_instant_action_ = std::bind(&VDA5050ClientAPI::instantActionWorker, this);
    this->client_ptr_->execute_parallel_action_ = std::bind(&VDA5050ClientAPI::excuteAction, this);

    this->client_ptr_->velocity_max_action_ = [this](double velocity)
    {
      this->updateVelocity(velocity);
    };

    this->client_ptr_->angular_max_action_ = [this](double angular)
    {
      this->updateAngular(angular);
    };

    std::string name;
    nh.param("base_global_planner", name, name);
    ros::NodeHandle nh_priv = ros::NodeHandle(nh, name);

    ros::NodeHandle nh_control = ros::NodeHandle("~");
    nh_control.param("global_plan_msg_type", global_plan_msg_type_, std::string("vda5050_msgs::Order"));
    if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
      VDA5050ClientAPI::plan_pub_ = nh_priv.advertise<nav_msgs::Path>("/plan_with_nav_path", 100);
    else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
    {
      std::string order_topic = nh_priv.getNamespace() + "/order";
      VDA5050ClientAPI::plan_pub_ = nh_priv.advertise<vda5050_msgs::Order>(order_topic, 100);
    }

    this->update_thread_ = std::thread(std::bind(&VDA5050ClientAPI::updating, this));
  }

  VDA5050ClientAPI::~VDA5050ClientAPI()
  {
    if (this->arm_thread_ptr_)
    {
      delete (arm_thread_ptr_);
    }

    if (this->belt_thread_ptr_)
    {
      delete (belt_thread_ptr_);
    }
    if (client_ptr_)
      client_ptr_.reset();
  }

  void VDA5050ClientAPI::orderWorker()
  {
    if (this->client_ptr_)
    {
      auto global_order = VDA5050ClientAPI::client_ptr_->vda5050_order_;

      std::string goal_maker;
      if (!global_order.nodes.empty())
      {                                                        // Kiểm tra map không rỗng
        auto lastNodeIt = std::prev(global_order.nodes.end()); // Lấy iterator phần tử cuối
        for (auto it = lastNodeIt->actions.begin(); it != lastNodeIt->actions.end(); ++it)
        {
          if (it->get()->actionType == "DockToStation")
            goal_maker = "DockToStation";
        }
      }
      uint8_t status;
      std::string message;
      if (goal_maker.empty())
        this->moveTo(global_order, status, message);
      else if (goal_maker == "DockToStation")
      {
        double theta = global_order.nodes.back().nodePosition.theta;
        global_order.edges.erase(global_order.edges.end());
        global_order.nodes.erase(global_order.nodes.end());
        global_order.nodes.back().nodePosition.theta = theta;

        if (!global_order.edges.empty() && (unsigned int)global_order.nodes.size())
          this->moveToDock(global_order, status, message);
        else
          this->rotateTo(global_order, status, message);
      }
    }
  }

  void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message)
  {
    robot_geometry_msgs::PoseStamped goal;
    if (VDA5050ClientAPI::move_base_ptr_)
    {
      if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
      {
        std::vector<vda_5050::Pose> paths;
        if (MakePlanWithOrder(order, false, status, message, paths))
        {
          ROS_INFO_STREAM("MakePlanWithOrder - move forward : " << "status: " << status << ", message: " << message);
        }
        else if (MakePlanWithOrder(order, true, status, message, paths))
        {
          ROS_INFO_STREAM("MakePlanWithOrder - move backward: " << "status: " << status << ", message: " << message);
        }
        else
        {
          ROS_WARN_STREAM("MakePlanWithOrder: return false: " << "status: " << status << ", message: " << message);
          this->client_ptr_->vda5050_state_.edgeStates.clear();
          this->client_ptr_->vda5050_state_.nodeStates.clear();
          this->client_ptr_->vda5050_state_.actionStates.clear();
          return;
        }
        if (!paths.empty())
          return;

        std::vector<robot_geometry_msgs::Pose2D> poses;
        for (auto &p : paths)
        {
          robot_geometry_msgs::Pose2D p_2d;
          p_2d.x = p.getX();
          p_2d.y = p.getY();
          p_2d.theta = p.getYaw();
          poses.push_back(p_2d);
        }
        const robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(poses, "map", robot::Time::now());
        goal = path.poses.back();
        // VDA5050ClientAPI::plan_pub_.publish(path);
        VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
      }
      else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
      {
        robot_protocol_msgs::Order order_msg;
        // vda5050_msgs::Order order_msg;
        order_msg.headerId = order.headerId;
        order_msg.timestamp = order.timestamp;
        order_msg.version = order.version;
        order_msg.manufacturer = order.manufacturer;
        order_msg.serialNumber = order.serialNumber;
        order_msg.orderId = order.orderId;
        order_msg.orderUpdateId = order.orderUpdateId;

        for (auto node : order.nodes)
        {
          robot_protocol_msgs::Node node_msg;
          node_msg.nodeId = node.nodeId;
          node_msg.sequenceId = node.sequenceId;
          node_msg.nodeDescription = node.nodeDescription;
          node_msg.released = node.released;

          node_msg.nodePosition.x = node.nodePosition.x;
          node_msg.nodePosition.y = node.nodePosition.y;
          node_msg.nodePosition.theta = node.nodePosition.theta;
          node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY;
          node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta;
          node_msg.nodePosition.mapId = node.nodePosition.mapId;
          node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription;
          // vda5050_msgs/Action[] actions
          order_msg.nodes.push_back(node_msg);
        }

        for (auto edge : order.edges)
        {
          robot_protocol_msgs::Edge edge_msg;
          edge_msg.edgeId = edge.edgeId;
          edge_msg.sequenceId = edge.sequenceId;
          edge_msg.edgeDescription = edge.edgeDescription;
          edge_msg.released = edge.released;
          edge_msg.startNodeId = edge.startNodeId;
          edge_msg.endNodeId = edge.endNodeId;
          edge_msg.maxSpeed = edge.maxSpeed;
          edge_msg.maxHeight = edge.maxHeight;
          edge_msg.minHeight = edge.minHeight;
          edge_msg.orientation = edge.orientation;
          edge_msg.orientationType = edge.orientationType;
          edge_msg.direction = edge.direction;
          edge_msg.rotationAllowed = edge.rotationAllowed;
          edge_msg.maxRotationSpeed = edge.maxRotationSpeed;
          edge_msg.length = edge.length;
          edge_msg.trajectory.degree = edge.trajectory.degree;
          edge_msg.trajectory.knotVector = edge.trajectory.knotVector;

          for (auto controlPoint : edge.trajectory.controlPoints)
          {
            robot_protocol_msgs::ControlPoint controlPoint_msg;
            controlPoint_msg.x = controlPoint.x;
            controlPoint_msg.y = controlPoint.y;
            controlPoint_msg.weight = controlPoint.weight;
            edge_msg.trajectory.controlPoints.push_back(controlPoint_msg);
          }
          // Corridor corridor;
          // std::vector<Action> actions;
          order_msg.edges.push_back(edge_msg);
        }

        vda_5050::NodePosition position = order.nodes.back().nodePosition;
        goal.header.stamp = robot::Time::now();
        goal.header.frame_id = "map";
        goal.pose.position.x = position.x;
        goal.pose.position.y = position.y;
        goal.pose.position.z = 0.0;

        // Convert theta (yaw) to Quaternion using setRPY
        // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used
        tf3::Quaternion quat;
        quat.setRPY(0.0, 0.0, position.theta);
        goal.pose.orientation.x = quat.x();
        goal.pose.orientation.y = quat.y();
        goal.pose.orientation.z = quat.z();
        goal.pose.orientation.w = quat.w();

        VDA5050ClientAPI::move_base_ptr_->moveTo(order_msg, goal);
      }
      else
        return;

      cmd_vel_max_.x = 0.2;
      cmd_vel_max_.theta = 0.5;
      cmd_vel_max_saved_ = cmd_vel_max_;

      robot_geometry_msgs::Vector3 linear;
      linear.x = cmd_vel_max_.x;
      VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
      linear.x = -cmd_vel_max_.x;
      VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);

      robot_geometry_msgs::Vector3 angular;
      angular.z = cmd_vel_max_.theta;
      VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
    }
  }

  void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message)
  {
    robot_geometry_msgs::PoseStamped goal;
    if (VDA5050ClientAPI::move_base_ptr_)
    {
      if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
      {
        std::vector<vda_5050::Pose> paths;
        if (MakePlanWithOrder(order, false, status, message, paths))
        {
          ROS_INFO_STREAM("MakePlanWithOrder - move forward : " << "status: " << status << ", message: " << message);
        }
        else if (MakePlanWithOrder(order, true, status, message, paths))
        {
          ROS_INFO_STREAM("MakePlanWithOrder - move backward: " << "status: " << status << ", message: " << message);
        }
        else
        {
          ROS_WARN_STREAM("MakePlanWithOrder: return false: " << "status: " << status << ", message: " << message);
          this->client_ptr_->vda5050_state_.edgeStates.clear();
          this->client_ptr_->vda5050_state_.nodeStates.clear();
          this->client_ptr_->vda5050_state_.actionStates.clear();
          return;
        }
        if (!paths.empty())
          return;

        std::vector<geometry_msgs::Pose2D> poses;
        for (auto &p : paths)
        {
          geometry_msgs::Pose2D p_2d;
          p_2d.x = p.getX();
          p_2d.y = p.getY();
          p_2d.theta = p.getYaw();
          poses.push_back(p_2d);
        }
        // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
        // goal = path.poses.back();
        // VDA5050ClientAPI::plan_pub_.publish(path);
      }
      else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
      {
        vda5050_msgs::Order order_msg;
        order_msg.headerId = order.headerId;
        order_msg.timestamp = order.timestamp;
        order_msg.version = order.version;
        order_msg.manufacturer = order.manufacturer;
        order_msg.serialNumber = order.serialNumber;
        order_msg.orderId = order.orderId;
        order_msg.orderUpdateId = order.orderUpdateId;

        for (auto node : order.nodes)
        {
          vda5050_msgs::Node node_msg;
          node_msg.nodeId = node.nodeId;
          node_msg.sequenceId = node.sequenceId;
          node_msg.nodeDescription = node.nodeDescription;
          node_msg.released = node.released;

          node_msg.nodePosition.x = node.nodePosition.x;
          node_msg.nodePosition.y = node.nodePosition.y;
          node_msg.nodePosition.theta = node.nodePosition.theta;
          node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY;
          node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta;
          node_msg.nodePosition.mapId = node.nodePosition.mapId;
          node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription;
          // vda5050_msgs/Action[] actions
          order_msg.nodes.push_back(node_msg);
        }

        for (auto edge : order.edges)
        {
          vda5050_msgs::Edge edge_msg;
          edge_msg.edgeId = edge.edgeId;
          edge_msg.sequenceId = edge.sequenceId;
          edge_msg.edgeDescription = edge.edgeDescription;
          edge_msg.released = edge.released;
          edge_msg.startNodeId = edge.startNodeId;
          edge_msg.endNodeId = edge.endNodeId;
          edge_msg.maxSpeed = edge.maxSpeed;
          edge_msg.maxHeight = edge.maxHeight;
          edge_msg.minHeight = edge.minHeight;
          edge_msg.orientation = edge.orientation;
          edge_msg.orientationType = edge.orientationType;
          edge_msg.direction = edge.direction;
          edge_msg.rotationAllowed = edge.rotationAllowed;
          edge_msg.maxRotationSpeed = edge.maxRotationSpeed;
          edge_msg.length = edge.length;
          edge_msg.trajectory.degree = edge.trajectory.degree;
          edge_msg.trajectory.knotVector = edge.trajectory.knotVector;

          for (auto controlPoint : edge.trajectory.controlPoints)
          {
            vda5050_msgs::ControlPoint controlPoint_msg;
            controlPoint_msg.x = controlPoint.x;
            controlPoint_msg.y = controlPoint.y;
            controlPoint_msg.weight = controlPoint.weight;
            edge_msg.trajectory.controlPoints.push_back(controlPoint_msg);
          }
          // Corridor corridor;
          // std::vector<Action> actions;
          order_msg.edges.push_back(edge_msg);
        }

        VDA5050ClientAPI::plan_pub_.publish(order_msg);

        vda_5050::NodePosition position = order.nodes.back().nodePosition;
        goal.header.stamp = robot::Time::now();
        goal.header.frame_id = "map";
        goal.pose.position.x = position.x;
        goal.pose.position.y = position.y;
        // tf3::Transform transform;
        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
        // goal = tf3::toMsg(transform);
      }
      else
        return;

      ros::Duration(0.1).sleep();
      VDA5050ClientAPI::move_base_ptr_->dockTo("trolley", goal, 0.2, 0.1);
      cmd_vel_max_.x = 0.2;
      cmd_vel_max_.theta = 0.5;
      cmd_vel_max_saved_ = cmd_vel_max_;

      robot_geometry_msgs::Vector3 linear;
      linear.x = cmd_vel_max_.x;
      VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
      linear.x = -cmd_vel_max_.x;
      VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);

      robot_geometry_msgs::Vector3 angular;
      angular.z = cmd_vel_max_.theta;
      VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
    }
  }

  void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message)
  {
    robot_geometry_msgs::PoseStamped goal;
    if (VDA5050ClientAPI::move_base_ptr_)
    {
      if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal))
      {
        vda_5050::NodePosition position = order.nodes.back().nodePosition;
        goal.header.stamp = robot::Time::now();
        goal.header.frame_id = "map";
        // tf3::Transform transform;
        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
        // goal = tf3::toMsg(transform);
        // VDA5050ClientAPI::move_base_ptr_->rotateTo(goal);
      }
      else
        return;
    }
  }

  void VDA5050ClientAPI::instantActionWorker()
  {

    if (this->client_ptr_)
    {
      auto &current_action_states = this->client_ptr_->vda5050_state_.actionStates;
      auto &current_actions = this->client_ptr_->vda5050_instant_action_.actions;

      for (auto action : current_actions)
      {
        auto same_action_state =
            std::find_if(current_action_states.begin(), current_action_states.end(),
                         [&action](const std::shared_ptr<vda_5050::ActionState> &as)
                         {
                           return action->actionType == as->actionType;
                         });
        if (same_action_state != current_action_states.end())
        {
          if (same_action_state->get()->actionStatus == vda_5050::ActionState::ActionStatus::INITIALIZING ||
              same_action_state->get()->actionStatus == vda_5050::ActionState::ActionStatus::RUNNING ||
              same_action_state->get()->actionStatus == vda_5050::ActionState::ActionStatus::WAITING)
            continue;
        }

        std::shared_ptr<vda_5050::ActionState> action_state = std::make_shared<vda_5050::ActionState>();
        action_state->actionDescription = action->actionDescription;
        action_state->actionId = action->actionId;
        action_state->actionType = action->actionType;
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING;

        *this->enable_action_ = true;
        *this->cancel_action_ = false;
        if (action_state->actionType == std::string("cancelOrder"))
        {
          current_action_states.push_back(action_state);
          this->cancelOrderCallBack(action_state);
        }
        else if (action_state->actionType.find("UnDockFromStation") != std::string::npos)
        {
          current_action_states.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::unDockFromStation, this, action, action_state);
        }
        else if (action_state->actionType == std::string("UnLoad"))
        {
          current_action_states.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::unLoad, this, action, action_state);
        }
        else if (action_state->actionType == std::string("Load"))
        {
          current_action_states.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::load, this, action, action_state);
        }
        else if (action_state->actionType == std::string("MutedOn"))
        {
          current_action_states.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyON, this, action, action_state);
        }
        else if (action_state->actionType == std::string("MutedOff"))
        {
          current_action_states.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyOFF, this, action, action_state);
        }
        else if (action_state->actionType == std::string("PickUp"))
        {
          current_action_states.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::pickUp, this, action, action_state);
        }
      }
    }
  }

  void VDA5050ClientAPI::excuteAction()
  {
    if (this->client_ptr_)
    {
      if (this->client_ptr_->parallel_execution_list_.empty())
        return;
      auto &curent_state = this->client_ptr_->vda5050_state_;
      for (auto &action : this->client_ptr_->parallel_execution_list_)
      {
        std::shared_ptr<vda_5050::ActionState> action_state = std::make_shared<vda_5050::ActionState>();
        action_state->actionDescription = action.second->actionDescription;
        action_state->actionId = action.second->actionId;
        action_state->actionType = action.second->actionType;
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING;

        if (action_state->actionType == std::string("MutedOn"))
        {
          curent_state.actionStates.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyON, this, action.second, action_state);
        }
        else if (action_state->actionType == std::string("MutedOff"))
        {
          curent_state.actionStates.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyOFF, this, action.second, action_state);
        }
        else if (action_state->actionType == std::string("DockToStation"))
        {
          curent_state.actionStates.push_back(action_state);
          this->excuteNoneAction(&VDA5050ClientAPI::dockToStaton, this, action.second, action_state);
        }
      }

      this->client_ptr_->parallel_execution_list_.erase(
          this->client_ptr_->parallel_execution_list_.begin(), this->client_ptr_->parallel_execution_list_.end());
    }
  }

  void VDA5050ClientAPI::updateVelocity(double velocity)
  {
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      if (nav_state == robot::move_base_core::State::CONTROLLING)
      {
        if (fabs(velocity) > 2.0)
          velocity = (fabs(velocity) / velocity) * 2.0;

        if (velocity != cmd_vel_max_saved_.x)
        {
          cmd_vel_max_.x = velocity;
          cmd_vel_max_saved_.x = cmd_vel_max_.x;
          ROS_INFO("Update velocity %f", velocity);
        }
      }
    }
  }

  void VDA5050ClientAPI::updateAngular(double angular)
  {
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      if (nav_state == robot::move_base_core::State::CONTROLLING)
      {
        if (fabs(angular) > 1.0)
          angular = (fabs(angular) / angular) * 1.0;

        if (angular != cmd_vel_max_saved_.theta)
        {
          cmd_vel_max_.theta = angular;
          cmd_vel_max_saved_.theta = cmd_vel_max_.theta;
          ROS_INFO("Update angular %f", angular);
        }
      }
    }
  }

  void VDA5050ClientAPI::cancelOrderCallBack(std::shared_ptr<vda_5050::ActionState> action_state)
  {
    if (this->client_ptr_)
    {
      // std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
      auto &global_state = this->client_ptr_->vda5050_state_;

      if (global_state.actionStates.empty())
        return;

      std::map<std::string, std::vector<std::shared_ptr<vda_5050::ActionState>>::iterator> other_cancel_action_state_mp;
      for (auto as = global_state.actionStates.begin(); as != global_state.actionStates.end(); ++as)
      {
        if (as->get()->actionType == "cancelOrder" && as->get()->actionId != action_state->actionId)
        {
          other_cancel_action_state_mp.insert({as->get()->actionId, as});
        }
      }

      if (action_state)
      {
        if (auto it = threads_map_.find(action_state->actionType); it != threads_map_.end())
        {
          if (it->second.joinable())
            it->second.join();
          std::lock_guard<std::mutex> lock(threads_mutex);
          VDA5050ClientAPI::threads_map_.erase(it);
        }
        if (!other_cancel_action_state_mp.empty())
        {
          bool prevours_order = false;
          for (auto other : other_cancel_action_state_mp)
          {
            if (other.second->get()->actionStatus == vda_5050::ActionState::ActionStatus::FINISHED ||
                other.second->get()->actionStatus == vda_5050::ActionState::ActionStatus::FAILED)
            {
              prevours_order = true;
            }
          }
          if (prevours_order)
          {
            for (auto other : other_cancel_action_state_mp)
            {
              global_state.actionStates.erase(other.second);
            }
          }
        }
        if (*this->cancel_action_)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "The previous cancelOrder was canceled";
          return;
        }

        ROS_WARN("Created action [%s]: %s", action_state->actionId.c_str(), action_state->actionType.c_str());
        VDA5050ClientAPI::threads_map_.insert({action_state->actionType,
                                               std::thread(&VDA5050ClientAPI::cancelOrder, this, action_state)});
      }
    }
  }

  void VDA5050ClientAPI::cancelOrder(std::shared_ptr<vda_5050::ActionState> action_state)
  {
    if (this->client_ptr_)
    {
      if (action_state->actionStatus == vda_5050::ActionState::ActionStatus::WAITING)
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
      // std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
      auto &global_state = this->client_ptr_->vda5050_state_;

      if (global_state.actionStates.size() < 1 && global_state.orderId.empty())
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "AGV has no order";
        vda_5050::Error error;
        error.errorType = "noOrderToCancel";
        error.errorLevel = vda_5050::Error::ErrorLevel::WARNING;
        error.errorDescription = "AGV has no order";
        global_state.errors.push_back(error);
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;

        // Can runing actions be interrupted ?
        if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
            VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
        {
          VDA5050ClientAPI::move_base_ptr_->cancel();
          *this->cancel_action_ = true;
          *this->enable_action_ = false;
          *this->pause_action_ = false;
          ros::Rate r(5);
          while (ros::ok() &&
                 VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state == robot::move_base_core::State::CONTROLLING)
          {
            ROS_INFO_THROTTLE(1.0, "Waiting to cancel");
            r.sleep();
            ros::spinOnce();
          }
          global_state.nodeStates.clear();
          global_state.edgeStates.clear();
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
          action_state->resultDescription = "cancelOrder is successed";
          // ros::Duration(0.5);
          // this->resetState();
        }
        else
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "AGV can not cancel when moving";
        }
      }
    }
  }

  template <typename T>
  void VDA5050ClientAPI::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)
  {
    if (this->client_ptr_)
    {
      // std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
      auto &global_state = this->client_ptr_->vda5050_state_;

      if (global_state.actionStates.empty())
        return;

      std::vector<std::vector<std::shared_ptr<vda_5050::ActionState>>::iterator> other_action_state_vt;
      for (auto as = global_state.actionStates.begin(); as != global_state.actionStates.end(); ++as)
      {
        if (as->get()->actionId == action_state->actionType &&
            (as->get()->actionStatus == vda_5050::ActionState::ActionStatus::FAILED || as->get()->actionStatus == vda_5050::ActionState::ActionStatus::FINISHED))
          other_action_state_vt.push_back(as);
      }
      if (!other_action_state_vt.empty())
      {
        for (auto &erase_as : other_action_state_vt)
          global_state.actionStates.erase(erase_as);
      }

      if (auto it = threads_map_.find(action_state->actionType); it != threads_map_.end())
      {
        if (it->second.joinable())
          it->second.join();
        std::lock_guard<std::mutex> lock(threads_mutex);
        VDA5050ClientAPI::threads_map_.erase(it);
      }
      VDA5050ClientAPI::threads_map_.insert({action_state->actionType, std::thread(excute, obj, action, action_state)});
    }
  }

  void VDA5050ClientAPI::unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
      ros::Rate r(5);
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      while (ros::ok() && !*this->cancel_action_ && nav_state == robot::move_base_core::State::CONTROLLING)
      {
        r.sleep();
        ros::spinOnce();
      }

      robot_geometry_msgs::PoseStamped pose;
      if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose))
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "AGV is lost localization";
        return;
      }
      auto distance_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(),
                                         [](const vda_5050::ActionParameter param)
                                         {
                                           return param.key == "distance";
                                         });
      double distance;
      if (distance_param != action->actionParameters.end())
      {
        try
        {
          distance = std::stod(distance_param->value);
        }
        catch (const std::invalid_argument &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Invalid argument: " << distance_param->key;
          action_state->resultDescription = ss.str();
          return;
        }
        catch (const std::out_of_range &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Out of range: " << distance_param->key;
          action_state->resultDescription = ss.str();
          return;
        }
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "No config 'distance' argument";
        return;
      }

      auto velocity_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(),
                                         [](const vda_5050::ActionParameter param)
                                         {
                                           return param.key == "velocity";
                                         });
      double velocity;
      if (velocity_param != action->actionParameters.end())
      {
        try
        {
          velocity = std::stod(velocity_param->value);
          if (fabs(velocity) > 0.3)
          {
            action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
            action_state->resultDescription = "'velocity' argument must be smaller than 0.3 (m/s)";
            return;
          }
        }
        catch (const std::invalid_argument &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Invalid argument: " << velocity_param->key;
          return;
        }
        catch (const std::out_of_range &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Out of range: " << velocity_param->key;
          return;
        }
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "No config 'velocity' argument";
        return;
      }

      robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
      VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal);
      robot_geometry_msgs::Vector3 linear;
      linear.x = fabs(velocity);
      VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
      linear.x = -fabs(velocity);
      VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);

      while (ros::ok() && !*this->cancel_action_)
      {
        if (nav_state == robot::move_base_core::State::CONTROLLING)
          break;
        r.sleep();
        ros::spinOnce();
      }

      while (ros::ok() && !*this->cancel_action_)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
        if (nav_state == robot::move_base_core::State::SUCCEEDED ||
            nav_state == robot::move_base_core::State::ABORTED ||
            nav_state == robot::move_base_core::State::PREEMPTED)
          break;
        r.sleep();
        ros::spinOnce();
      }

      if (nav_state == robot::move_base_core::State::SUCCEEDED)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
        action_state->resultDescription = "Done";
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "Failed by cancel order";
      }
      // this->resetState();
    }
  }

  void VDA5050ClientAPI::pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      ros::Rate r(5);
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
      {
        r.sleep();
        ros::spinOnce();
      }

      auto count_ok_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(),
                                         [](const vda_5050::ActionParameter param)
                                         {
                                           return param.key == "count_ok";
                                         });
      unsigned int count_ok;
      if (count_ok_param != action->actionParameters.end())
      {
        try
        {
          count_ok = std::stod(count_ok_param->value);
        }
        catch (const std::invalid_argument &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Invalid argument: " << count_ok_param->key;
          action_state->resultDescription = ss.str();
          return;
        }
        catch (const std::out_of_range &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Out of range: " << count_ok_param->key;
          action_state->resultDescription = ss.str();
          return;
        }
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "No config 'count_ok' argument";
        return;
      }

      auto count_ng_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(),
                                         [](const vda_5050::ActionParameter param)
                                         {
                                           return param.key == "count_ng";
                                         });
      double count_ng;
      if (count_ng_param != action->actionParameters.end())
      {
        try
        {
          count_ng = std::stod(count_ng_param->value);
        }
        catch (const std::invalid_argument &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Invalid argument: " << count_ng_param->key;
          return;
        }
        catch (const std::out_of_range &e)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          std::stringstream ss;
          ss << "Out of range: " << count_ng_param->key;
          return;
        }
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "No config 'count_ng' argument";
        return;
      }

      action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
      *count_ok_max_ = count_ok;
      *count_ng_max_ = count_ng;
      if (!this->arm_function_ptr_)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "Config is error";
      }
      if (this->arm_thread_ptr_ && this->arm_thread_ptr_->joinable())
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "Arm is busy";
      }
      else if (this->arm_thread_ptr_ && !this->arm_thread_ptr_->joinable())
      {
        ROS_INFO("Dobot is running...");
        this->arm_function_ptr_();

        ros::Rate r(5);
        while (ros::ok() && !*this->cancel_action_)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
          if (!this->arm_thread_ptr_->joinable())
            break;
          r.sleep();
          ros::spinOnce();
        }
        if (*this->cancel_action_)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "Failed by cancel order";
        }
      }
      else
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "Config is error";
      }

      action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
      action_state->resultDescription = "Done";
      // this->resetState();
    }
  }

  void VDA5050ClientAPI::dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
      ros::Rate r(5);
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
        r.sleep();
        ros::spinOnce();
      }
      if (nav_state == robot::move_base_core::State::SUCCEEDED)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
        action_state->resultDescription = "Done";
      }
      else
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
      // this->resetState();
    }
  }

  void VDA5050ClientAPI::unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    ROS_INFO("UnLoad running");
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      ros::Rate r(5);
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
      {
        if (nav_state == robot::move_base_core::State::ABORTED ||
            nav_state == robot::move_base_core::State::REJECTED)
        {
          if (action_state != nullptr)
          {
            action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
            action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
          }
          return;
        }
        if (nav_state == robot::move_base_core::State::SUCCEEDED)
          break;
        r.sleep();
      }

      if (!this->unLoad_excuted_)
      {
        if (action_state != nullptr)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "Config Error";
        }
        return;
      }
      if (this->belt_thread_ptr_ && this->belt_thread_ptr_->joinable())
      {
        if (action_state != nullptr)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "The belt is not be Finished by before action";
        }
      }
      else if (this->belt_thread_ptr_ && !this->belt_thread_ptr_->joinable())
      {
        this->unLoad_excuted_();
      }
      else
      {
        if (action_state != nullptr)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "Config Error";
        }
      }
      while (ros::ok() && !*this->cancel_action_)
      {
        if (action_state != nullptr)
          action_state->actionStatus = static_cast<vda_5050::ActionState::ActionStatus::ActionStatus_EN>(*this->cur_belt_state_en_);
        if (*this->cur_belt_state_en_ == amr_control::State::FAILED || *this->cur_belt_state_en_ == amr_control::State::FINISHED)
        {
          break;
        }
        r.sleep();
        ros::spinOnce();
      }
      if (*this->cancel_action_)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "Failed by cancel order";
      }
      else if (action_state != nullptr)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
        action_state->resultDescription = "Done";
      }
      // this->resetState();
    }
  }

  void VDA5050ClientAPI::load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    ROS_INFO("Load running");
    if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      ros::Rate r(5);
      auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
      {
        if (nav_state == robot::move_base_core::State::ABORTED ||
            nav_state == robot::move_base_core::State::REJECTED)
        {
          if (action_state != nullptr)
          {
            action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
            action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
          }
          return;
        }
        if (nav_state == robot::move_base_core::State::SUCCEEDED)
          break;
        r.sleep();
      }

      if (!this->load_excuted_)
      {
        if (action_state != nullptr)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
          action_state->resultDescription = "Config Error";
        }
        return;
      }
      if (this->belt_thread_ptr_ && this->belt_thread_ptr_->joinable())
      {
        if (action_state != nullptr)
        {
          action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
          action_state->resultDescription = "The belt is not be Finished by before action";
        }
      }
      else if (this->belt_thread_ptr_ && !this->belt_thread_ptr_->joinable())
      {
        this->load_excuted_();
      }

      while (ros::ok() && !*this->cancel_action_)
      {
        if (action_state != nullptr)
          action_state->actionStatus = static_cast<vda_5050::ActionState::ActionStatus::ActionStatus_EN>(*this->cur_belt_state_en_);
        if (*this->cur_belt_state_en_ == amr_control::State::FAILED || *this->cur_belt_state_en_ == amr_control::State::FINISHED)
        {
          break;
        }
        r.sleep();
        ros::spinOnce();
      }
      if (*this->cancel_action_)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
        action_state->resultDescription = "Failed by cancel order";
      }
      else if (action_state != nullptr)
      {
        action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
        action_state->resultDescription = "Done";
      }
      // this->resetState();
    }
  }

  void VDA5050ClientAPI::mutedSafetyON(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    *this->muted_value_ = true;
    action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
    action_state->resultDescription = "Muted is On";
  }

  void VDA5050ClientAPI::mutedSafetyOFF(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
  {
    *this->muted_value_ = false;
    action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
    action_state->resultDescription = "Muted is Off";
  }

  void VDA5050ClientAPI::updating()
  {
    ros::Rate r(10);
    while (ros::ok)
    {
      if (this->client_ptr_)
      {
        this->updatingvda5050Visualization();
        this->updatingvda5050State();
      }
      r.sleep();
      ros::spinOnce();
    }
  }

  void VDA5050ClientAPI::updatingvda5050Visualization()
  {
    std::lock_guard<std::mutex> lock(this->client_ptr_->visualization_mutex);

    auto &global_visualization = this->client_ptr_->vda5050_visualization_;
    if (VDA5050ClientAPI::move_base_ptr_)
    {
      robot_geometry_msgs::Pose2D robot_pose;
      if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
      {
        global_visualization.agvPosition.x = robot_pose.x;
        global_visualization.agvPosition.y = robot_pose.y;
        global_visualization.agvPosition.theta = robot_pose.theta;
      }
    }

    if (VDA5050ClientAPI::monitor_ptr_)
    {
      nav_2d_msgs::Twist2D velocity;
      if (VDA5050ClientAPI::monitor_ptr_->getVelocity(velocity))
      {
        global_visualization.velocity.vx = std::round(velocity.x * 1000.0) / 1000.0;
        global_visualization.velocity.vy = std::round(velocity.y * 1000.0) / 1000.0;
        global_visualization.velocity.omega = std::round(velocity.theta * 1000.0) / 1000.0;
      }
    }
  }

  void VDA5050ClientAPI::updatingvda5050State()
  {
    std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
    auto &global_state = this->client_ptr_->vda5050_state_;
    if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
        VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
    {
      robot_geometry_msgs::Pose2D robot_pose;
      if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
      {
        global_state.agvPosition.x = robot_pose.x;
        global_state.agvPosition.y = robot_pose.y;
        global_state.agvPosition.theta = robot_pose.theta;
      }

      robot::move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
      if (nav_state == robot::move_base_core::State::CONTROLLING)
      {
        global_state.driving = true;
        global_state.paused = false;
        global_state.newBaseRequest = false;
      }
      else if (nav_state == robot::move_base_core::State::PAUSED)
      {
        global_state.driving = false;
        global_state.paused = true;
        global_state.newBaseRequest = false;
      }
      else
      {
        global_state.driving = false;
        global_state.paused = false;
        global_state.newBaseRequest = true;
      }
    }

    global_state.orderId = this->client_ptr_->vda5050_order_.orderId;
    global_state.orderUpdateId = this->client_ptr_->vda5050_order_.orderUpdateId;
    global_state.operatingMode = this->mode_;
  }

  void VDA5050ClientAPI::resetState()
  {
    *this->cancel_action_ = false;
    *this->enable_action_ = false;
    *this->pause_action_ = false;
  }

  std::shared_ptr<robot::move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr;
  std::shared_ptr<loc_core::BaseLocalization> VDA5050ClientAPI::loc_base_ptr_ = nullptr;
  std::shared_ptr<AmrMonitor> VDA5050ClientAPI::monitor_ptr_ = nullptr;
}