#include "amr_comunication/vda_5050/vda_5050_connector.h"
#include "amr_comunication/vda_5050/utils/line_common.h"
#include <thread>

amr_comunication::VDA5050Connector::VDA5050Connector()
{
  try
  {
    this->userParams_ = std::make_unique<UserParams>();
    ros::NodeHandle nh_priv;
    nh_priv.param("MQTT/Name", userParams_->name_, userParams_->name_);
    nh_priv.param("MQTT/Host", userParams_->mqtt_hostname_, userParams_->mqtt_hostname_);
    nh_priv.param("MQTT/Port", userParams_->mqtt_port_, userParams_->mqtt_port_);
    nh_priv.param("MQTT/Client_ID", userParams_->mqtt_client_id_, userParams_->mqtt_client_id_);
    nh_priv.param("MQTT/Username", userParams_->user_name_, userParams_->user_name_);
    nh_priv.param("MQTT/Password", userParams_->password_, userParams_->password_);
    nh_priv.param("MQTT/Keep_Alive", userParams_->mqtt_keepalive_, userParams_->mqtt_keepalive_);

    this->mqtt_init();
    this->mqtt_connect();
    this->publish_mqtt_thread_ = std::thread(&amr_comunication::VDA5050Connector::updatingProgress, this);
  }
  catch (const std::exception &e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}

amr_comunication::VDA5050Connector::~VDA5050Connector()
{
  this->mqtt_cleanup();
  if (this->userParams_)
    this->userParams_.reset();
}

std::string amr_comunication::VDA5050Connector::getCurrentTimestamp()
{
  auto now = std::chrono::system_clock::now();

  auto in_time_t = std::chrono::system_clock::to_time_t(now);

  auto microseconds = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;

  std::stringstream ss;
  ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%dT%H:%M:%S");

  ss << '.' << std::setfill('0') << std::setw(6) << microseconds.count();

  return ss.str();
}

void amr_comunication::VDA5050Connector::mqtt_init()
{
  mosquitto_lib_init();
  this->mosq_ = mosquitto_new(userParams_->mqtt_client_id_.c_str(), true, NULL);
  if (!this->mosq_)
  {
    throw std::runtime_error("Failed to create mosquitto instance.");
    ROS_ERROR_STREAM("Failed to create mosquitto instance.");
  }
  mosquitto_message_callback_set(this->mosq_, &amr_comunication::VDA5050Connector::mqtt_message_callback);
  mosquitto_connect_callback_set(this->mosq_, &amr_comunication::VDA5050Connector::mqtt_on_connect_callback);
  mosquitto_disconnect_callback_set(this->mosq_, &amr_comunication::VDA5050Connector::mqtt_on_disconnect_callback);
}

void amr_comunication::VDA5050Connector::mqtt_cleanup()
{
  mosquitto_destroy(this->mosq_);
  mosquitto_lib_cleanup();
}

void amr_comunication::VDA5050Connector::mqtt_connect()
{
  ROS_INFO("Connecting mqtt broker ...");
  int login = mosquitto_username_pw_set(this->mosq_, userParams_->user_name_.c_str(), userParams_->password_.c_str());
  if (login == MOSQ_ERR_INVAL)
    throw std::runtime_error("Unable to login to broker. The input parameters were invalid");

  if (login == MOSQ_ERR_NOMEM)
    throw std::runtime_error("Unable to login to broker. An out of memory condition occurred.");

  if (mosquitto_connect(this->mosq_, userParams_->mqtt_hostname_.c_str(), userParams_->mqtt_port_, userParams_->mqtt_keepalive_))
  {
    ROS_ERROR("Unable to connect to broker.");
    std::this_thread::sleep_for(std::chrono::seconds(5));
    this->mqtt_reconnect(userParams_->mqtt_hostname_, userParams_->mqtt_port_);
    // throw std::runtime_error("Unable to connect to broker.");
  }

  mosquitto_loop_start(this->mosq_);
}

void amr_comunication::VDA5050Connector::mqtt_reconnect(const std::string &new_host, int new_port)
{
  ROS_INFO("Reconnecting to new MQTT broker at %s:%d ...", new_host.c_str(), new_port);

  // Stop the loop
  mosquitto_loop_stop(this->mosq_, true); // true = finish current messages
  mosquitto_disconnect(this->mosq_);

  // Optionally, set new credentials here if needed again
  int login = mosquitto_username_pw_set(this->mosq_, userParams_->user_name_.c_str(), userParams_->password_.c_str());
  if (login != MOSQ_ERR_SUCCESS)
    throw std::runtime_error("Failed to set username/password: " + std::string(mosquitto_strerror(login)));

  // Connect to the new host and port
  int rc = mosquitto_connect(this->mosq_, new_host.c_str(), new_port, 60);
  if (rc != MOSQ_ERR_SUCCESS)
  {
    // throw std::runtime_error("Failed to connect to broker: " + std::string(mosquitto_strerror(rc)));
    ROS_ERROR("Failed to connect to broker: %s", std::string(mosquitto_strerror(rc)).c_str());
    std::this_thread::sleep_for(std::chrono::seconds(5));
    this->mqtt_reconnect(userParams_->mqtt_hostname_, userParams_->mqtt_port_);
  }

  // Start the loop again
  rc = mosquitto_loop_start(this->mosq_);
  if (rc != MOSQ_ERR_SUCCESS)
    throw std::runtime_error("Failed to start Mosquitto loop: " + std::string(mosquitto_strerror(rc)));
}

void amr_comunication::VDA5050Connector::mqtt_on_connect_callback(struct mosquitto *mosq, void *userdata, int result)
{
  amr_comunication::VDA5050Connector *client = static_cast<amr_comunication::VDA5050Connector *>(userdata);
  if (result == 0)
  {
    ROS_INFO_STREAM("Connected to MQTT broker successfully!");
    // Subscribe lại topic sau khi kết nối lại thành công
    mosquitto_subscribe(mosq, NULL, "order", 0);
    mosquitto_subscribe(mosq, NULL, "instantActions", 0);
  }
  else
  {
    ROS_ERROR_STREAM("Failed to connect: " << mosquitto_strerror(result));
  }
}

void amr_comunication::VDA5050Connector::mqtt_on_disconnect_callback(struct mosquitto *mosq, void *userdata, int rc)
{
  amr_comunication::VDA5050Connector *client = static_cast<amr_comunication::VDA5050Connector *>(userdata);
  if (rc != 0)
  {
    ROS_ERROR_STREAM("Unexpected disconnect, reconnecting...");
    std::this_thread::sleep_for(std::chrono::seconds(5));
    mosquitto_reconnect(mosq);
  }
  else
  {
    ROS_INFO_STREAM("Disconnected from broker cleanly.");
  }
}

void amr_comunication::VDA5050Connector::mqtt_message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message)
{
  amr_comunication::VDA5050Connector *client = static_cast<amr_comunication::VDA5050Connector *>(userdata);
  if (message->payloadlen)
  {
    std::string payload(static_cast<char *>(message->payload), message->payloadlen);
    std::string mqtt_topic(message->topic);
    if (mqtt_topic == "order")
    {
      ROS_INFO("order");
      vda_5050::Order received_order;
      // All formatting and JSON data types are correct?
      bool is_received_order_vaild = client->ParseOrderMsg(payload, received_order);
      if (!is_received_order_vaild)
        return;

      if (received_order.manufacturer != "phenikaaX")
        return;

      if (received_order.serialNumber != userParams_->mqtt_client_id_)
        return;

      if (!amr_comunication::VDA5050Connector::acceptingOrderOrOrderUpdate(received_order, amr_comunication::VDA5050Connector::vda5050_order_))
      {
        ROS_ERROR("Reject order");
      }
      else
      {
        if (amr_comunication::VDA5050Connector::execute_order_)
          amr_comunication::VDA5050Connector::execute_order_();
      }
    }
    else if (mqtt_topic == "instantActions")
    {
      ROS_INFO("instantActions");
      vda_5050::InstantAction action;
      if (!client->ParseInstantActionMsg(payload, action))
        return;
      if (action.manufacturer != "phenikaaX")
        return;
      if (action.serialNumber != userParams_->mqtt_client_id_)
        return;

      if (!amr_comunication::VDA5050Connector::acceptingInstantActions(
              action, amr_comunication::VDA5050Connector::vda5050_instant_action_))
      {
        ROS_ERROR("Reject instantAtion");
      }
      else
      {
        if (amr_comunication::VDA5050Connector::execute_instant_action_)
          amr_comunication::VDA5050Connector::execute_instant_action_();
      }
    }
  }
  else
  {
    ROS_INFO_STREAM("Other message received");
  }
}

void amr_comunication::VDA5050Connector::publishMqttTask()
{
  // nav_2d_msgs::Pose2DStamped robot_pose_on_map;
  userParams_->mqtt_client_id_ = userParams_->mqtt_client_id_;
  vda5050_visualization_.serialNumber = userParams_->mqtt_client_id_;
  vda5050_visualization_.timestamp = this->getCurrentTimestamp();
  nlohmann::json virsualization_js = this->vda5050VisualizationToJson(this->vda5050_visualization_);
  std::string visualization_payload = virsualization_js.dump();
  if (this->mosq_ != nullptr)
  {
    int pub_ret1 = mosquitto_publish(this->mosq_, NULL, "visualization", (int)visualization_payload.length(), visualization_payload.c_str(), 0, false);
  }
  if (vda5050_visualization_.headerId > (UINT32_MAX - 1))
  {
    vda5050_visualization_.headerId = 0;
  }
  else
  {
    vda5050_visualization_.headerId++;
  }

  vda5050_state_.serialNumber = userParams_->mqtt_client_id_;
  vda5050_state_.timestamp = this->getCurrentTimestamp();
  nlohmann::json state_js = this->vda5050StateToJson(this->vda5050_state_);

  std::string state_payload = state_js.dump();
  if (this->mosq_ != nullptr)
  {
    int pub_ret2 = mosquitto_publish(this->mosq_, NULL, "state", (int)state_payload.length(), state_payload.c_str(), 0, false);
  }
  if (vda5050_state_.headerId > (UINT32_MAX - 1))
  {
    vda5050_state_.headerId = 0;
  }
  else
  {
    vda5050_state_.headerId++;
  }
}

void amr_comunication::VDA5050Connector::updatingProgress()
{
  ros::Rate r(50);
  ros::Time publish_time = ros::Time::now();
  while (ros::ok())
  {
    this->stateInProgress();

    if ((ros::Time::now() - publish_time) >= ros::Duration(0.1))
    {
      this->publishMqttTask();
      publish_time = ros::Time::now();
    }
    r.sleep();
    ros::spinOnce();
  }
}

void amr_comunication::VDA5050Connector::stateInProgress()
{
  auto cache_state = amr_comunication::VDA5050Connector::vda5050_state_;
  auto cache_order = amr_comunication::VDA5050Connector::vda5050_order_;
  if (!cache_state.driving)
  {
    if (cache_state.nodeStates.size() == 1 && cache_state.edgeStates.empty())
    {
      std::string end_node_state_id = cache_state.nodeStates.begin()->nodeId;
      auto node_excute = std::find_if(cache_order.nodes.begin(), cache_order.nodes.end(),
                                      [&end_node_state_id](const vda_5050::Node &n)
                                      {
                                        ROS_INFO("node[%s] [%s]", end_node_state_id.c_str(), n.nodeId.c_str());
                                        return end_node_state_id == n.nodeId;
                                      });
      if (node_excute != cache_order.nodes.end())
      {
        ROS_INFO("Excute node[%s]", node_excute->nodeDescription.c_str());
        for (auto action : node_excute->actions)
        {
          if (parallel_execution_list_.find(action->actionId) == parallel_execution_list_.end())
          {
            parallel_execution_list_.insert({action->actionId, action});
            ROS_INFO("%s %s", action->actionId.c_str(), action->actionType.c_str());
          }
        }
        if (amr_comunication::VDA5050Connector::execute_parallel_action_)
        {
          amr_comunication::VDA5050Connector::execute_parallel_action_();
        }
      }
      // std::lock_guard<std::mutex> lock(state_mutex);
      auto &global_state = amr_comunication::VDA5050Connector::vda5050_state_;
      global_state.nodeStates.erase(global_state.nodeStates.begin(), global_state.nodeStates.end());
    }
  }
  else
  {
    if (!cache_state.nodeStates.empty() && !cache_state.edgeStates.empty() && (int)cache_state.nodeStates.size() > 1)
    {
      std::map<std::string, std::vector<vda_5050::EdgeState>::iterator> edge_state_map;
      for (auto egde_state = cache_state.edgeStates.begin(); egde_state != cache_state.edgeStates.end(); ++egde_state)
      {
        edge_state_map.insert({egde_state->edgeId, egde_state});
      }

      std::map<std::string, std::vector<vda_5050::NodeState>::iterator> node_state_map;
      for (auto node_state = cache_state.nodeStates.begin(); node_state != cache_state.nodeStates.end(); ++node_state)
      {
        node_state_map.insert({node_state->nodeId, node_state});
      }

      std::map<std::string, std::vector<vda_5050::Edge>::iterator> edge_map;
      for (auto edge = cache_order.edges.begin(); edge != cache_order.edges.end(); ++edge)
      {
        edge_map.insert({edge->edgeId, edge});
      }

      std::map<std::string, std::vector<vda_5050::Node>::iterator> node_map;
      for (auto node = cache_order.nodes.begin(); node != cache_order.nodes.end(); ++node)
      {
        node_map.insert({node->nodeId, node});
      }

      double distance_to_start = std::numeric_limits<double>::infinity();
      bool started_path = false;
      std::vector<vda_5050::EdgeState>::iterator edge_state_close;
      Spline_Inf *input_spline_inf = new Spline_Inf();
      CurveCommon *curve_design = new CurveCommon();

      // Toa do hien tai
      auto position = cache_state.agvPosition;
      for (auto edge_state = cache_state.edgeStates.begin(); edge_state != cache_state.edgeStates.end(); ++edge_state)
      {
        std::vector<vda_5050::Pose> poses_on_edge;
        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> control_points;
        std::vector<double> knot_vector;
        std::vector<double> weight_vector;
        int degree = 0;
        int order_ = 0;
        control_points.reserve(edge_state->trajectory.controlPoints.size());
        knot_vector.reserve(edge_state->trajectory.knotVector.size());
        weight_vector.reserve(edge_state->trajectory.controlPoints.size());
        for (auto ctr_point : edge_state->trajectory.controlPoints)
        {
          control_points.push_back(Eigen::Vector3d(ctr_point.x, ctr_point.y, 0));
          weight_vector.push_back(ctr_point.weight);
        }
        for (auto knotVector : edge_state->trajectory.knotVector)
        {
          knot_vector.push_back(knotVector);
        }
        degree = (int)edge_state->trajectory.degree;
        if (curve_design->curveIsValid(degree, knot_vector, control_points))
        {
          double step = 0.01;
          order_ = degree + 1;
          input_spline_inf->control_point.clear();
          input_spline_inf->knot_vector.clear();
          input_spline_inf->weight.clear();
          curve_design->ReadSplineInf(input_spline_inf, order_, control_points, knot_vector);
          curve_design->ReadSplineInf(input_spline_inf, weight_vector, false);
          for (double u_test = 0; u_test <= 1; u_test += step)
          {
            geometry_msgs::Point curve_point;
            curve_point = curve_design->CalculateCurvePoint(input_spline_inf, u_test, true);
            if (!std::isnan(curve_point.x) && !std::isnan(curve_point.y))
              poses_on_edge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0));
          }

          for (auto pose : poses_on_edge)
          {
            const double distance = std::hypot((pose.getX() - position.x), (pose.getY() - position.y));
            if (distance_to_start > distance)
            {
              distance_to_start = distance;
              edge_state_close = edge_state;
              started_path = true;
            }
          }
        }
      }
      delete (input_spline_inf);
      delete (curve_design);

      if (!started_path)
      {
        ROS_ERROR_NAMED("VDA5050Connector", "None of the node of the edge were in the order.");
        if (velocity_max_action_)
          velocity_max_action_(0.3);

        if (angular_max_action_)
          angular_max_action_(0.3);
      }
      else
      {
        if (auto search_edge = edge_map.find(edge_state_close->edgeId); search_edge != edge_map.end())
        {
          auto start_node_m = node_map.find(search_edge->second->startNodeId);
          auto end_node_m = node_map.find(search_edge->second->endNodeId);
          if (start_node_m == node_map.end() || end_node_m == node_map.end())
          {
            ROS_WARN_STREAM_THROTTLE(1.0, "Not found end_node_m\n");
            return;
          }
          // ROS_INFO_STREAM_THROTTLE(1.0, "Found " << start_node_m->second->nodeDescription << ' ' << end_node_m->second->nodeDescription);

          auto position = cache_state.agvPosition;
          const double start_dx = start_node_m->second->nodePosition.x - position.x;
          const double start_dy = start_node_m->second->nodePosition.y - position.y;
          const double end_dx = end_node_m->second->nodePosition.x - position.x;
          const double end_dy = end_node_m->second->nodePosition.y - position.y;

          auto start_node_state_m = node_state_map.find(start_node_m->second->nodeId);
          auto end_node_state_m = node_state_map.find(end_node_m->second->nodeId);
          auto edge_state_m = edge_state_map.find(search_edge->second->edgeId);
          if (start_node_state_m != node_state_map.end() && end_node_state_m != node_state_map.end() && edge_state_m != edge_state_map.end())
          {
            // std::lock_guard<std::mutex> lock(state_mutex);
            auto &global_state = amr_comunication::VDA5050Connector::vda5050_state_;
            if (auto edge_order = edge_map.find(edge_state_m->first); edge_order != edge_map.end())
            {
              if (velocity_max_action_)
                velocity_max_action_((double)edge_order->second->maxSpeed);

              if (angular_max_action_)
                angular_max_action_((double)edge_order->second->maxRotationSpeed);
            }
            else
            {
              if (velocity_max_action_)
                velocity_max_action_(0.3);
              if (angular_max_action_)
                angular_max_action_(0.3);
            }

            if (std::hypot(end_dx, end_dy) <= end_node_m->second->nodePosition.allowedDeviationXY)
            {
              auto node_state_erase = std::find_if(global_state.nodeStates.begin(), global_state.nodeStates.end(),
                                                   [&start_node_state_m](const vda_5050::NodeState &ns)
                                                   {
                                                     return start_node_state_m->second->nodeId == ns.nodeId;
                                                   });

              if (node_state_erase != global_state.nodeStates.end())
                global_state.nodeStates.erase(node_state_erase);

              auto edge_state_erase = std::find_if(global_state.edgeStates.begin(), global_state.edgeStates.end(),
                                                   [&edge_state_m](const vda_5050::EdgeState &ns)
                                                   {
                                                     return edge_state_m->second->edgeId == ns.edgeId;
                                                   });

              if (edge_state_erase != global_state.edgeStates.end())
                global_state.edgeStates.erase(edge_state_erase);

              auto node_state_excute = std::find_if(global_state.nodeStates.begin(), global_state.nodeStates.end(),
                                                    [&end_node_state_m](const vda_5050::NodeState &ns)
                                                    {
                                                      return end_node_state_m->second->nodeId == ns.nodeId;
                                                    });

              if (node_state_erase != global_state.nodeStates.end() &&
                  edge_state_erase != global_state.edgeStates.end() &&
                  node_state_excute != global_state.nodeStates.end())
              {
                global_state.lastNodeId = end_node_m->second->nodeId;
                global_state.lastNodeSequenceId++;
                global_state.distanceSinceLastNode = std::hypot(end_dx, end_dy);

                auto &global_order = amr_comunication::VDA5050Connector::vda5050_order_;
                auto node_excute = std::find_if(global_order.nodes.begin(), global_order.nodes.end(),
                                                [&node_state_excute](const vda_5050::Node &n)
                                                {
                                                  return node_state_excute->nodeId == n.nodeId;
                                                });
                if (node_excute != global_order.nodes.end())
                {
                  for (auto action : node_excute->actions)
                  {
                    if (parallel_execution_list_.find(action->actionId) == parallel_execution_list_.end())
                    {
                      parallel_execution_list_.insert({action->actionId, action});
                    }
                  }
                  if (amr_comunication::VDA5050Connector::execute_parallel_action_)
                  {
                    amr_comunication::VDA5050Connector::execute_parallel_action_();
                  }
                }
              }
            }
            else
            {
              // std::lock_guard<std::mutex> lock(state_mutex);
              auto &global_state = amr_comunication::VDA5050Connector::vda5050_state_;
              global_state.lastNodeId = start_node_m->second->nodeId;
              global_state.distanceSinceLastNode = std::hypot(start_dx, start_dy);
            }
          }
        }
        else
          ROS_ERROR_STREAM_THROTTLE(1.0, "Not found search_edge\n");
      }
    }
  }
}

void amr_comunication::VDA5050Connector::orderMsgStream(vda_5050::Order order)
{
  std::stringstream msg;
  msg << "\n"
      << "headerId: " << order.headerId << "\n"
      << "timestamp: " << order.timestamp << "\n"
      << "version: " << order.version << "\n"
      << "manufacturer: " << order.manufacturer << "\n"
      << "serialNumber: " << order.serialNumber << "\n"
      << "orderId: " << order.orderId << "\n"
      << "orderUpdateId: " << order.orderUpdateId << "\n"
      << "zoneSetId: " << order.zoneSetId << "\n";

  msg << "Nodes[" << (int)order.nodes.size() << "]: \n";
  for (auto &n : order.nodes)
  {
    msg << "  " << "nodeId: " << n.nodeId << "\n"
        << "  " << "sequenceId: " << n.sequenceId << "\n"
        << "  " << "nodeDescription: " << n.nodeDescription << "\n"
        << "  " << "released: " << n.released << "\n"
        << "  " << "nodePosition: " << "\n";

    msg << "    " << "x: " << n.nodePosition.x << "\n"
        << "    " << "y: " << n.nodePosition.y << "\n"
        << "    " << "theta: " << n.nodePosition.theta << "\n"
        << "    " << "allowedDeviationXY: " << n.nodePosition.allowedDeviationXY << "\n"
        << "    " << "allowedDeviationTheta: " << n.nodePosition.allowedDeviationTheta << "\n"
        << "    " << "mapId: " << n.nodePosition.mapId << "\n"
        << "    " << "mapDescription: " << n.nodePosition.mapDescription << "\n";

    msg << "    " << "actions[" << (int)n.actions.size() << "]: \n";
    for (auto &action : n.actions)
    {
      msg << "      " << "actionType: " << action->actionType << "\n"
          << "      " << "actionId: " << action->actionId << "\n"
          << "      " << "actionDescription: " << action->actionDescription << "\n"
          << "      " << "blockingType: " << action->blockingType << "\n";
      msg << "      " << "actionParameters[" << (int)action->actionParameters.size() << "]: \n";
      for (auto &apt : action->actionParameters)
      {
        msg << "        " << "key: " << apt.key << "\n"
            << "        " << "value: " << apt.value << "\n";
      }
    }
    msg << std::endl;
  }
  ROS_INFO_STREAM(msg.str());
}

bool amr_comunication::VDA5050Connector::ParseOrderMsg(const std::string &payload, vda_5050::Order &order)
{
  nlohmann::json j;
  try
  {
    j = nlohmann::json::parse(payload);
  }
  catch (const nlohmann::json::parse_error &e)
  {
    ROS_ERROR_STREAM("JSON parse error: " << e.what() << std::endl);
    ROS_INFO("ParseOrderMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::invalid_iterator &e)
  {
    ROS_ERROR_STREAM("JSON invalid iterator: " << e.what() << std::endl);
    ROS_INFO("ParseOrderMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::type_error &e)
  {
    ROS_ERROR_STREAM("JSON type error: " << e.what() << std::endl);
    ROS_INFO("ParseOrderMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::out_of_range &e)
  {
    ROS_ERROR_STREAM("JSON out of range: " << e.what() << std::endl);
    ROS_INFO("ParseOrderMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::other_error &e)
  {
    ROS_ERROR_STREAM("JSON other error: " << e.what() << std::endl);
    ROS_INFO("ParseOrderMsg : \n%s", payload.c_str());
    return false;
  }

  // Parse header info
  if (!this->checkObjectStructure<int>(j, "headerId"))
    return false;
  order.headerId = j["headerId"];

  if (!this->checkObjectStructure<std::string>(j, "timestamp"))
    return false;
  order.timestamp = j["timestamp"];

  if (!this->checkObjectStructure<std::string>(j, "version"))
    return false;
  order.version = j["version"];

  if (!this->checkObjectStructure<std::string>(j, "manufacturer"))
    return false;
  order.manufacturer = j["manufacturer"];

  if (!this->checkObjectStructure<std::string>(j, "serialNumber"))
    return false;
  order.serialNumber = j["serialNumber"];

  if (!this->checkObjectStructure<std::string>(j, "orderId"))
    return false;
  order.orderId = j["orderId"];

  if (!this->checkObjectStructure<int>(j, "orderUpdateId"))
    return false;
  order.orderUpdateId = j["orderUpdateId"];

  if (!this->checkObjectStructure<std::string>(j, "zoneSetId"))
    return false;
  order.zoneSetId = j["zoneSetId"];

  if (!j["nodes"].is_array())
  {
    ROS_WARN("'nodes' is not array type");
    return false;
  }
  // Parse nodes
  for (const auto &nodeJson : j["nodes"])
  {
    vda_5050::Node node;
    if (!this->checkObjectStructure<std::string>(nodeJson, "nodeId"))
      return false;
    node.nodeId = nodeJson["nodeId"];

    if (!this->checkObjectStructure<int>(nodeJson, "sequenceId"))
      return false;
    node.sequenceId = nodeJson["sequenceId"];

    if (!this->checkObjectStructure<std::string>(nodeJson, "nodeDescription"))
      return false;
    node.nodeDescription = nodeJson["nodeDescription"];

    if (!this->checkObjectStructure<bool>(nodeJson, "released"))
      return false;
    node.released = nodeJson["released"];

    // Parse node position
    if (!this->checkObjectStructure<vda_5050::NodePosition>(nodeJson, "nodePosition"))
      return false;

    auto nodePosition = nodeJson["nodePosition"];

    if (!this->checkObjectStructure<double>(nodePosition, "x"))
      return false;
    node.nodePosition.x = nodePosition["x"];

    if (!this->checkObjectStructure<double>(nodePosition, "y"))
      return false;
    node.nodePosition.y = nodePosition["y"];

    if (!this->checkObjectStructure<double>(nodePosition, "theta"))
      return false;
    node.nodePosition.theta = nodePosition["theta"];

    if (!this->checkObjectStructure<double>(nodePosition, "allowedDeviationXY"))
      return false;
    node.nodePosition.allowedDeviationXY = nodePosition["allowedDeviationXY"];

    if (!this->checkObjectStructure<double>(nodePosition, "allowedDeviationTheta"))
      return false;
    node.nodePosition.allowedDeviationTheta = nodePosition["allowedDeviationTheta"];

    if (!this->checkObjectStructure<std::string>(nodePosition, "mapId"))
      return false;
    node.nodePosition.mapId = nodePosition["mapId"];

    if (!this->checkObjectStructure<std::string>(nodePosition, "mapDescription"))
      return false;
    node.nodePosition.mapDescription = nodePosition["mapDescription"];

    if (!nodeJson["actions"].is_array())
    {
      ROS_WARN("'actions' is not array type");
      return false;
    }
    // Parse actions
    for (const auto &actionJson : nodeJson["actions"])
    {
      vda_5050::Action action;
      if (!this->checkObjectStructure<std::string>(actionJson, "actionType"))
        return false;
      action.actionType = actionJson["actionType"];

      if (!this->checkObjectStructure<std::string>(actionJson, "actionId"))
        return false;
      action.actionId = actionJson["actionId"];

      if (!this->checkObjectStructure<std::string>(actionJson, "actionDescription"))
        return false;
      action.actionDescription = actionJson["actionDescription"];

      if (!this->checkObjectStructure<std::string>(actionJson, "blockingType"))
        return false;
      action.blockingType = actionJson["blockingType"];

      if (!actionJson["actionParameters"].is_array())
      {
        ROS_WARN("'actionParameters' is not array type");
        return false;
      }

      // Parse action parameters
      for (const auto &paramJson : actionJson["actionParameters"])
      {
        vda_5050::ActionParameter param;
        if (!this->checkObjectStructure<std::string>(paramJson, "key"))
          return false;
        param.key = paramJson["key"];

        if (!this->checkObjectStructure<std::string>(paramJson, "value"))
          return false;
        param.value = paramJson["value"];

        action.actionParameters.push_back(param);
      }
      node.actions.push_back(std::make_shared<vda_5050::Action>(action));
    }
    order.nodes.push_back(node);
  }

  if (!j["edges"].is_array())
  {
    ROS_WARN("'edges' is not array type");
    return false;
  }
  // Parse edges
  for (const auto &edgeJson : j["edges"])
  {
    vda_5050::Edge edge;
    if (!this->checkObjectStructure<std::string>(edgeJson, "edgeId"))
      return false;
    edge.edgeId = edgeJson["edgeId"];

    if (!this->checkObjectStructure<int>(edgeJson, "sequenceId"))
      return false;
    edge.sequenceId = edgeJson["sequenceId"];

    if (!this->checkObjectStructure<std::string>(edgeJson, "edgeDescription"))
      return false;
    edge.edgeDescription = edgeJson["edgeDescription"];

    if (!this->checkObjectStructure<bool>(edgeJson, "released"))
      return false;
    edge.released = edgeJson["released"];

    if (!this->checkObjectStructure<std::string>(edgeJson, "startNodeId"))
      return false;
    edge.startNodeId = edgeJson["startNodeId"];

    if (!this->checkObjectStructure<std::string>(edgeJson, "endNodeId"))
      return false;
    edge.endNodeId = edgeJson["endNodeId"];

    if (!this->checkObjectStructure<double>(edgeJson, "maxSpeed"))
      return false;
    edge.maxSpeed = edgeJson["maxSpeed"];

    if (!this->checkObjectStructure<double>(edgeJson, "maxHeight"))
      return false;
    edge.maxHeight = edgeJson["maxHeight"];

    if (!this->checkObjectStructure<double>(edgeJson, "minHeight"))
      return false;
    edge.minHeight = edgeJson["minHeight"];

    if (!this->checkObjectStructure<double>(edgeJson, "orientation"))
      return false;
    edge.orientation = edgeJson["orientation"];

    if (!this->checkObjectStructure<std::string>(edgeJson, "orientationType"))
      return false;
    edge.orientationType = edgeJson["orientationType"];

    if (!this->checkObjectStructure<std::string>(edgeJson, "direction"))
      return false;
    edge.direction = edgeJson["direction"];

    if (!this->checkObjectStructure<bool>(edgeJson, "rotationAllowed"))
      return false;
    edge.rotationAllowed = edgeJson["rotationAllowed"];

    if (!this->checkObjectStructure<double>(edgeJson, "maxRotationSpeed"))
      return false;
    edge.maxRotationSpeed = edgeJson["maxRotationSpeed"];

    if (!this->checkObjectStructure<double>(edgeJson, "length"))
      return false;
    edge.length = edgeJson["length"];

    // Parse trajectory
    if (!this->checkObjectStructure<vda_5050::Trajectory>(edgeJson, "trajectory"))
      return false;
    auto trajectoryJson = edgeJson["trajectory"];

    if (!this->checkObjectStructure<int>(trajectoryJson, "degree"))
      return false;
    edge.trajectory.degree = trajectoryJson["degree"];

    if (!trajectoryJson["knotVector"].is_array())
    {
      ROS_WARN("'knotVector' is not array type");
      return false;
    }
    for (const auto &knot : trajectoryJson["knotVector"])
    {
      if (!knot.is_number_float() && !knot.is_number_integer())
      {
        ROS_WARN("'knot' is not double type");
        return false;
      }
      edge.trajectory.knotVector.push_back(knot);
    }

    // Parse control points
    if (!trajectoryJson["controlPoints"].is_array())
    {
      ROS_WARN("'controlPoints' is not double type");
      return false;
    }
    for (const auto &pointJson : trajectoryJson["controlPoints"])
    {
      vda_5050::ControlPoint point;
      if (!this->checkObjectStructure<double>(pointJson, "x"))
        return false;
      point.x = pointJson["x"];

      if (!this->checkObjectStructure<double>(pointJson, "y"))
        return false;
      point.y = pointJson["y"];

      if (!this->checkObjectStructure<double>(pointJson, "weight"))
        return false;
      point.weight = pointJson["weight"];

      edge.trajectory.controlPoints.push_back(point);
    }

    order.edges.push_back(edge);
  }
  return true;
}

bool amr_comunication::VDA5050Connector::ParseInstantActionMsg(const std::string &payload, vda_5050::InstantAction &instantAction)
{
  nlohmann::json j;
  try
  {
    j = nlohmann::json::parse(payload);
  }
  catch (const nlohmann::json::parse_error &e)
  {
    ROS_ERROR_STREAM("JSON parse error: " << e.what() << std::endl);
    ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::invalid_iterator &e)
  {
    ROS_ERROR_STREAM("JSON invalid iterator: " << e.what() << std::endl);
    ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::type_error &e)
  {
    ROS_ERROR_STREAM("JSON type error: " << e.what() << std::endl);
    ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::out_of_range &e)
  {
    ROS_ERROR_STREAM("JSON out of range: " << e.what() << std::endl);
    ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str());
    return false;
  }
  catch (const nlohmann::json::other_error &e)
  {
    ROS_ERROR_STREAM("JSON other error: " << e.what() << std::endl);
    ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str());
    return false;
  }

  // Parse header info
  if (!this->checkObjectStructure<int>(j, "headerId"))
    return false;
  instantAction.headerId = j["headerId"];

  if (!this->checkObjectStructure<std::string>(j, "timestamp"))
    return false;
  instantAction.timestamp = j["timestamp"];

  if (!this->checkObjectStructure<std::string>(j, "version"))
    return false;
  instantAction.version = j["version"];

  if (!this->checkObjectStructure<std::string>(j, "manufacturer"))
    return false;
  instantAction.manufacturer = j["manufacturer"];

  if (!this->checkObjectStructure<std::string>(j, "serialNumber"))
    return false;
  instantAction.serialNumber = j["serialNumber"];

  // Parse actions
  if (!j["actions"].is_array())
  {
    ROS_WARN("'actions' is not array type");
    return false;
  }
  for (const auto &actionJson : j["actions"])
  {
    vda_5050::Action action;
    if (!this->checkObjectStructure<std::string>(actionJson, "actionType"))
      return false;
    action.actionType = actionJson["actionType"];

    if (!this->checkObjectStructure<std::string>(actionJson, "actionId"))
      return false;
    action.actionId = actionJson["actionId"];

    if (!this->checkObjectStructure<std::string>(actionJson, "actionDescription"))
      return false;
    action.actionDescription = actionJson["actionDescription"];

    if (!this->checkObjectStructure<std::string>(actionJson, "blockingType"))
      return false;
    action.blockingType = actionJson["blockingType"];

    if (!actionJson["actionParameters"].is_array())
    {
      ROS_WARN("'actionParameters' is not array type");
      return false;
    }
    // Parse action parameters
    for (const auto &paramJson : actionJson["actionParameters"])
    {
      vda_5050::ActionParameter param;
      if (!this->checkObjectStructure<std::string>(paramJson, "key"))
        return false;
      param.key = paramJson["key"];

      if (!this->checkObjectStructure<std::string>(paramJson, "value"))
        return false;
      param.value = paramJson["value"];

      action.actionParameters.push_back(param);
    }
    instantAction.actions.push_back(std::make_shared<vda_5050::Action>(action));
  }

  return true;
}

nlohmann::json amr_comunication::VDA5050Connector::vda5050StateToJson(const vda_5050::State &state)
{
  nlohmann::json j;

  j["headerId"] = state.headerId;
  j["timestamp"] = state.timestamp;
  j["version"] = state.version;
  j["manufacturer"] = state.manufacturer;
  j["serialNumber"] = state.serialNumber;
  j["orderId"] = state.orderId;
  j["orderUpdateId"] = state.orderUpdateId;
  j["zoneSetId"] = state.zoneSetId;
  j["lastNodeId"] = state.lastNodeId;
  j["lastNodeSequenceId"] = state.lastNodeSequenceId;

  // Convert nodeStates
  for (const auto &nodeState : state.nodeStates)
  {
    j["nodeStates"].push_back({
        {"nodeId", nodeState.nodeId},
        {"sequenceId", nodeState.sequenceId},
        {"nodeDescription", nodeState.nodeDescription},
        {"released", nodeState.released},
        {"nodePosition", {{"x", nodeState.nodePosition.x}, {"y", nodeState.nodePosition.y}, {"theta", nodeState.nodePosition.theta}, {"allowedDeviationXY", nodeState.nodePosition.allowedDeviationXY}, {"allowedDeviationTheta", nodeState.nodePosition.allowedDeviationTheta}, {"mapId", nodeState.nodePosition.mapId}, {"mapDescription", nodeState.nodePosition.mapDescription}}},
    });
  }

  // Convert edgeStates
  for (const auto &edgeState : state.edgeStates)
  {
    j["edgeStates"].push_back({{"edgeId", edgeState.edgeId},
                               {"sequenceId", edgeState.sequenceId},
                               {"edgeDescription", edgeState.edgeDescription},
                               {"released", edgeState.released},
                               {"trajectory", {{"degree", edgeState.trajectory.degree}, {"knotVector", edgeState.trajectory.knotVector}, {"controlPoints", nlohmann::json::array()}}}});

    for (const auto &controlPoint : edgeState.trajectory.controlPoints)
    {
      j["edgeStates"].back()["trajectory"]["controlPoints"].push_back({{"x", controlPoint.x},
                                                                       {"y", controlPoint.y},
                                                                       {"weight", controlPoint.weight}});
    }
  }

  j["agvPosition"] = {
      {"x", state.agvPosition.x},
      {"y", state.agvPosition.y},
      {"theta", state.agvPosition.theta},
      {"mapId", state.agvPosition.mapId},
      {"positionInitialized", state.agvPosition.positionInitialized},
      {"localizationScore", state.agvPosition.localizationScore},
      {"deviationRange", state.agvPosition.deviationRange}};

  j["velocity"] = {
      {"vx", state.velocity.vx},
      {"vy", state.velocity.vy},
      {"omega", state.velocity.omega}};

  // Convert loads
  for (const auto &load : state.loads)
  {
    j["loads"].push_back({{"loadId", load.loadId},
                          {"loadType", load.loadType},
                          {"loadPosition", load.loadPosition},
                          {"boundingBoxReference", {{"x", load.boundingBoxReference.x}, {"y", load.boundingBoxReference.y}, {"z", load.boundingBoxReference.z}, {"theta", load.boundingBoxReference.theta}}},
                          {"loadDimensions", {{"length", load.loadDimensions.length}, {"width", load.loadDimensions.width}, {"height", load.loadDimensions.height}}},
                          {"weight", load.weight}});
  }

  j["driving"] = state.driving;
  j["paused"] = state.paused;
  j["newBaseRequest"] = state.newBaseRequest;
  j["distanceSinceLastNode"] = state.distanceSinceLastNode;
  j["operatingMode"] = state.operatingMode._toString();

  j["batteryState"] = {
      {"batteryCharge", state.batteryState.batteryCharge},
      {"batteryVoltage", state.batteryState.batteryVoltage},
      {"batteryHealth", state.batteryState.batteryHealth},
      {"charging", state.batteryState.charging},
      {"reach", state.batteryState.reach}};

  j["safetyState"] = {
      {"eStop", state.safetyState.eStop},
      {"fieldViolation", state.safetyState.fieldViolation}};

  // Convert actionStates
  for (const auto &actionState : state.actionStates)
  {
    j["actionStates"].push_back({{"actionID", actionState->actionId},
                                 {"actionType", actionState->actionType},
                                 {"actionDescription", actionState->actionDescription},
                                 {"actionStatus", actionState->actionStatus._toString()},
                                 {"resultDescription", actionState->resultDescription}});
  }

  // Convert errors
  for (const auto &error : state.errors)
  {
    nlohmann::json errorJson = {
        {"errorType", error.errorType},
        {"errorDescription", error.errorDescription},
        {"errorLevel", error.errorLevel._toString()}};

    // Convert errorReferences
    for (const auto &ref : error.errorReferences)
    {
      errorJson["errorReferences"].push_back({{"referenceKey", ref.referenceKey},
                                              {"referenceValue", ref.referenceValue}});
    }

    j["errors"].push_back(errorJson);
  }

  // Convert information
  for (const auto &info : state.information)
  {
    nlohmann::json infoJson = {
        {"infoType", info.infoType},
        {"infoDescription", info.infoDescription},
        {"infoLevel", info.infoLevel}};

    // Convert infoReferences
    for (const auto &ref : info.infoReferences)
    {
      infoJson["infoReferences"].push_back({{"referenceKey", ref.referenceKey},
                                            {"referenceValue", ref.referenceValue}});
    }

    j["information"].push_back(infoJson);
  }

  return j;
}

nlohmann::json amr_comunication::VDA5050Connector::vda5050VisualizationToJson(const vda_5050::Visualization &visualization)
{
  nlohmann::json j;

  j["headerId"] = visualization.headerId;
  j["timestamp"] = visualization.timestamp;
  j["version"] = visualization.version;
  j["manufacturer"] = visualization.manufacturer;
  j["serialNumber"] = visualization.serialNumber;

  j["agvPosition"] = {
      {"x", visualization.agvPosition.x},
      {"y", visualization.agvPosition.y},
      {"theta", visualization.agvPosition.theta},
      {"mapId", visualization.agvPosition.mapId},
      {"positionInitialized", visualization.agvPosition.positionInitialized},
      {"localizationScore", visualization.agvPosition.localizationScore},
      {"deviationRange", visualization.agvPosition.deviationRange}};

  j["velocity"] = {
      {"vx", visualization.velocity.vx},
      {"vy", visualization.velocity.vy},
      {"omega", visualization.velocity.omega}};

  return j;
}

template <typename T>
bool amr_comunication::VDA5050Connector::checkObjectStructure(nlohmann::json json, std::string obj)
{
  if (!json.contains(obj))
  {
    ROS_WARN("Object structure '%s' is not existed", obj.c_str());
    return false;
  }

  if constexpr (std::is_same<T, int>::value)
  {
    if (!json[obj].is_number_integer())
    {
      ROS_WARN("Object '%s' is not an integer", obj.c_str());
      return false;
    }
  }
  else if constexpr (std::is_same<T, double>::value)
  {
    if (!json[obj].is_number_float() && !json[obj].is_number_integer())
    {
      ROS_WARN("Object '%s' is not a float or integer", obj.c_str());
      return false;
    }
  }
  else if constexpr (std::is_same<T, std::string>::value)
  {
    if (!json[obj].is_string())
    {
      ROS_WARN("Object '%s' is not a string", obj.c_str());
      return false;
    }
  }
  else if constexpr (std::is_same<T, bool>::value)
  {
    if (!json[obj].is_boolean())
    {
      ROS_WARN("Object '%s' is not a boolean", obj.c_str());
      return false;
    }
  }
  else if constexpr (std::is_class<T>::value)
  {
    if (!json[obj].is_object())
    {
      ROS_WARN("Object '%s' is not a struct", obj.c_str());
      return false;
    }
  }
  else
  {
    ROS_WARN("Unknown type check for object '%s'", obj.c_str());
    return false;
  }
  return true;
}

bool amr_comunication::VDA5050Connector::acceptingOrderOrOrderUpdate(const vda_5050::Order &received_order, vda_5050::Order &current_order)
{
  bool is_received_order_new;
  // is received order new or an update of the current order?
  // Is orderId of the received order different to orderId of order the vehicle currently holds?
  if (received_order.orderId == current_order.orderId)
  {
  }
  else
  {
    /**
     * is vehicle still executing an order or waiting for an update?
     * Are nodeStates not empty or are actionStates containing states which are neither 'FAILED' nor 'FINISHED'?
     * Nodes and edges and the corresponding action states of the order horizon are also included inside the state.
     * Vehicle might still have a horizon and therefore waiting for an update and executing an order.
     */
    bool is_vehicle_still_executing_an_order =
        amr_comunication::VDA5050Connector::isTheVehicleStillExecuting();
    bool waiting_for_an_update =
        amr_comunication::VDA5050Connector::isTheVehicleWaitingAnUpdate();

    auto &errors = amr_comunication::VDA5050Connector::vda5050_state_.errors;
    auto error = std::find_if(errors.begin(), errors.end(),
                              [](const vda_5050::Error e)
                              {
                                return e.errorType == std::string("RejectOrder");
                              });
    if (error != errors.end())
    {
      errors.erase(error);
    }

    if (is_vehicle_still_executing_an_order)
    {
      vda_5050::Error e;
      e.errorDescription = "Is AGV still executing an order or waiting for an update?";
      e.errorLevel = vda_5050::Error::ErrorLevel::WARNING;
      e.errorType = "RejectOrder";
      errors.push_back(e);
    }

    if (is_vehicle_still_executing_an_order || waiting_for_an_update)
    {
      ROS_INFO("Is vehicle still executing an order or waiting for an update? %x %x ", is_vehicle_still_executing_an_order, waiting_for_an_update);
      return false;
    }
    /**
     * Is start of new order close enough to current position?
     * Is the vehicle already standing on the node, or is it in the node's deviation range (see Section 6.6.1 Concept and logic)?
     */
    bool is_start_of_new_order_close_enough_to_current_position =
        amr_comunication::VDA5050Connector::isAGVOnNodeOrInDeviationRangeOfNewOrder(received_order);

    // if (!is_start_of_new_order_close_enough_to_current_position)
    // {
    //   ROS_INFO("Is start of new order close enough to current position?");
    //   return false;
    // }

    // accept order
    current_order = received_order;
    // std::lock_guard<std::mutex> lock(state_mutex);
    auto &current_state = amr_comunication::VDA5050Connector::vda5050_state_;
    amr_comunication::VDA5050Connector::populateNodeStates(current_order, current_state);
    amr_comunication::VDA5050Connector::populateEdgeStates(current_order, current_state);
    amr_comunication::VDA5050Connector::populateActionStates(current_order, current_state);
    current_state.lastNodeSequenceId = 0;
    current_state.lastNodeId = "";
  }
  return true;
}

bool amr_comunication::VDA5050Connector::acceptingInstantActions(
    const vda_5050::InstantAction &received_instantAction, vda_5050::InstantAction &current_instantAction)
{
  if (received_instantAction.actions.empty())
    return false;
  current_instantAction = received_instantAction;
  return true;
}

bool amr_comunication::VDA5050Connector::isTheVehicleStillExecuting()
{
  auto &current_state = amr_comunication::VDA5050Connector::vda5050_state_;

  // Are nodeStates not empty ?
  bool is_not_node_state_empty = !current_state.nodeStates.empty();

  // Are actionStates containing states which are neither 'FAILED' nor 'FINISHED'?
  bool is_action_running = false;
  for (auto &action_state : current_state.actionStates)
  {
    if (action_state->actionStatus != vda_5050::ActionState::ActionStatus::FINISHED &&
        action_state->actionStatus != vda_5050::ActionState::ActionStatus::FAILED)
    {
      is_action_running = true;
      break;
    }
  }
  return is_not_node_state_empty || is_action_running;
}

bool amr_comunication::VDA5050Connector::isTheVehicleWaitingAnUpdate()
{
  auto &current_state = amr_comunication::VDA5050Connector::vda5050_state_;
  return current_state.getTypeOrder() == vda_5050::OrderType::Horizon;
}

bool amr_comunication::VDA5050Connector::isAGVOnNodeOrInDeviationRange()
{
  auto &current_state = amr_comunication::VDA5050Connector::vda5050_state_;
  if (current_state.nodeStates.empty())
    return false;

  for (const auto &node : current_state.nodeStates)
  {
    double dx = current_state.agvPosition.x - node.nodePosition.x;
    double dy = current_state.agvPosition.y - node.nodePosition.y;
    double distance = std::sqrt(dx * dx + dy * dy);

    // Check if AGV is exactly on the node OR within deviation range
    if (distance <= node.nodePosition.allowedDeviationXY)
    {
      return true;
    }
  }
  return false;
}

bool amr_comunication::VDA5050Connector::isAGVOnNodeOrInDeviationRangeOfNewOrder(vda_5050::Order new_order)
{
  // std::lock_guard<std::mutex> lock(state_mutex);
  auto &current_state = amr_comunication::VDA5050Connector::vda5050_state_;
  if (new_order.nodes.empty())
    return false;

  double distance_min = std::numeric_limits<double>::infinity();
  double allowed_xy;
  std::string node_str;
  for (const auto &node : new_order.nodes)
  {
    double dx = current_state.agvPosition.x - node.nodePosition.x;
    double dy = current_state.agvPosition.y - node.nodePosition.y;
    double distance = std::sqrt(dx * dx + dy * dy);

    // Check if AGV is exactly on the node OR within deviation range
    if (distance <= fabs(node.nodePosition.allowedDeviationXY))
    {
      return true;
    }

    if (distance <= distance_min)
    {
      distance_min = distance;
      allowed_xy = node.nodePosition.allowedDeviationXY;
      node_str = std::string(node.nodeId.c_str());
    }
  }
  ROS_INFO("AGV distance %f to %s with allowedxy %f", distance_min, node_str.c_str(), allowed_xy);
  return false;
}

void amr_comunication::VDA5050Connector::populateNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state)
{
  curent_state.nodeStates.clear();
  for (auto &node : receiverd_order.nodes)
  {
    vda_5050::NodeState node_state;
    node_state.nodeId = node.nodeId;
    node_state.sequenceId = node.sequenceId;
    node_state.nodeDescription = node.nodeDescription;
    node_state.released = node_state.released;
    node_state.nodePosition = node.nodePosition;
    curent_state.nodeStates.push_back(node_state);
  }
}

void amr_comunication::VDA5050Connector::appendNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state)
{
}

void amr_comunication::VDA5050Connector::populateEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state)
{
  curent_state.edgeStates.clear();
  int count = 0;
  for (auto &edge : receiverd_order.edges)
  {
    vda_5050::EdgeState edge_state;
    edge_state.edgeId = edge.edgeId;
    edge_state.sequenceId = edge.sequenceId;
    edge_state.edgeDescription = edge.edgeDescription;
    edge_state.released = edge.released;
    edge_state.trajectory = edge.trajectory;
    curent_state.edgeStates.push_back(edge_state);
    ROS_INFO("State->EdgeState[%d]: %s", count, edge_state.edgeDescription.c_str());
    count++;
  }
  ROS_INFO(" ");
}

void amr_comunication::VDA5050Connector::appendEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state)
{
}

void amr_comunication::VDA5050Connector::populateActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state)
{
  // std::lock_guard<std::mutex> lock(state_mutex);
  curent_state.actionStates.clear();
  // for (auto &edge : receiverd_order.edges)
  // {
  //   auto start_node = std::find_if(receiverd_order.nodes.begin(), receiverd_order.nodes.end(),
  //                                  [&edge](const vda_5050::Node &n)
  //                                  {
  //                                    return edge.startNodeId == n.nodeId;
  //                                  });

  //   auto end_node = std::find_if(receiverd_order.nodes.begin(), receiverd_order.nodes.end(),
  //                                [&edge](const vda_5050::Node &n)
  //                                {
  //                                  return edge.endNodeId == n.nodeId;
  //                                });

  //   if (start_node != receiverd_order.nodes.end())
  //   {
  //     if (!start_node->actions.empty())
  //     {
  //       for (auto action : start_node->actions)
  //       {
  //         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;
  //         curent_state.actionStates.push_back(action_state);
  //       }
  //     }
  //   }

  //   for (auto action : edge.actions)
  //   {
  //     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;
  //     curent_state.actionStates.push_back(action_state);
  //   }

  //   if (end_node != receiverd_order.nodes.end())
  //   {
  //     if (!end_node->actions.empty())
  //     {
  //       for (auto action : end_node->actions)
  //       {
  //         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;
  //         curent_state.actionStates.push_back(action_state);
  //       }
  //     }
  //   }
  // }
  // int count = 0;
  // for (auto action : curent_state.actionStates)
  // {
  //   ROS_INFO("State->ActionState[%d]: %s", count, action->actionType.c_str());
  //   count++;
  // }
  // ROS_INFO(" ");
}

void amr_comunication::VDA5050Connector::appendActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state)
{
}

std::unique_ptr<amr_comunication::VDA5050Connector::UserParams> amr_comunication::VDA5050Connector::userParams_;
vda_5050::Order amr_comunication::VDA5050Connector::vda5050_order_;
vda_5050::InstantAction amr_comunication::VDA5050Connector::vda5050_instant_action_;
vda_5050::State amr_comunication::VDA5050Connector::vda5050_state_;
vda_5050::Visualization amr_comunication::VDA5050Connector::vda5050_visualization_;
std::mutex amr_comunication::VDA5050Connector::state_mutex;
std::mutex amr_comunication::VDA5050Connector::visualization_mutex;

std::function<void()> amr_comunication::VDA5050Connector::execute_order_;
std::function<void()> amr_comunication::VDA5050Connector::execute_instant_action_;
std::function<void()> amr_comunication::VDA5050Connector::execute_parallel_action_;
std::function<void(double)> amr_comunication::VDA5050Connector::velocity_max_action_;
std::function<void(double)> amr_comunication::VDA5050Connector::angular_max_action_;