#include "amr_control/amr_make_plan_with_order.h"

namespace amr_control
{
  bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t &status, std::string &message, std::vector<vda_5050::Pose> &posesOnPathWay)
  {
    std::map<std::string, vda_5050::Node> orderNodes;
    Spline_Inf *input_spline_inf = new Spline_Inf();
    CurveCommon *curve_design = new CurveCommon();
    posesOnPathWay.clear();
    std::map<std::string, vda_5050::Node> map_str;
    if ((int)order.nodes.size() == 0 || (int)order.edges.size() == 0)
    {
      status = 1;
      message = "Nodes or Edges in Order is empty";
      return false;
    }
    for (auto &node : order.nodes)
    {
      orderNodes.insert({node.nodeId, node});
    }
    for (int i = 0; i < (int)order.edges.size(); i++)
    {
      auto start_nodeId_it = orderNodes.find(order.edges[i].startNodeId);
      auto end_nodeId_it = orderNodes.find(order.edges[i].endNodeId);

      if (start_nodeId_it != orderNodes.end() && end_nodeId_it != orderNodes.end())
      {
        std::vector<vda_5050::Pose> posesOnEdge;
        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(order.edges[i].trajectory.controlPoints.size());
        knot_vector.reserve(order.edges[i].trajectory.knotVector.size());
        weight_vector.reserve(order.edges[i].trajectory.controlPoints.size());
        for (int j = 0; j < (int)order.edges[i].trajectory.controlPoints.size(); j++)
        {
          control_points.push_back(Eigen::Vector3d(order.edges[i].trajectory.controlPoints[j].x, order.edges[i].trajectory.controlPoints[j].y, 0));
          weight_vector.push_back(order.edges[i].trajectory.controlPoints[j].weight);
        }
        for (int k = 0; k < (int)order.edges[i].trajectory.knotVector.size(); k++)
        {
          knot_vector.push_back(order.edges[i].trajectory.knotVector[k]);
        }
        degree = (int)order.edges[i].trajectory.degree;
        if (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))
              posesOnEdge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0));
            // ROS_INFO("curve_point: %f, %f   at u: %f",curve_point.x, curve_point.y, u_test);
          }
          if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
          {
            // if startNode of this edge and start element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
            {
              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
            }
            // if endNode of this edge and end element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
            {
              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
            }
            if (!posesOnPathWay.empty())
            {
              if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626)
              {
                setYawAllPosesOnEdge(posesOnEdge, false);
              }
              else if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276)
              {
                setYawAllPosesOnEdge(posesOnEdge, true);
              }
              else
              {
                ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
                status = 3;
                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                return false;
                break;
              }
            }
            else
            {
              if (is_move_backward == false)
              {
                setYawAllPosesOnEdge(posesOnEdge, false);
              }
              else
              {
                setYawAllPosesOnEdge(posesOnEdge, true);
              }
            }
            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
            {
              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
              {
                if (i != 1)
                {                                                                                                                                   // don't check angle of edge 1
                  if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
                  {
                    // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
                    posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
                    posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
                  }
                  else
                  {
                    ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                             order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
                    status = 3;
                    message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                              ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                    return false;
                    break;
                  }
                }
                else
                {
                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
                }
              }
              else
              {
                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
                status = 3;
                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                return false;
                break;
              }
            }
            else // posesOnPathWay is empty
            {
              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
            }
          }
          else if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
          {
            // if startNode of this edge and start element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
            {
              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
            }
            // if endNode of this edge and end element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
            {
              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
            }
            if (i == ((int)order.edges.size() - 1))
            {
              if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 1.5707963268) // <= 90 degree
              {
                setYawAllPosesOnEdge(posesOnEdge, false);
                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
              }
              else
              {
                setYawAllPosesOnEdge(posesOnEdge, true);
                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
              }
            }
            else
            {
              if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626) // <= 50 degree
              {
                setYawAllPosesOnEdge(posesOnEdge, false);
                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
              }
              else if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276) // >= 130 degree
              {
                setYawAllPosesOnEdge(posesOnEdge, true);
                posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
              }
              else
              {
                ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
                status = 3;
                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                return false;
                break;
              }
            }
            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
            {
              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
              {
                if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
                {
                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
                }
                else
                {
                  ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                           order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
                  status = 3;
                  message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                            ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                  return false;
                  break;
                }
              }
              else
              {
                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
                status = 3;
                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                return false;
                break;
              }
            }
            else // posesOnPathWay is empty
            {
              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
            }
          }
          else if (isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
          {
            // if startNode of this edge and start element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
            {
              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
            }
            // if endNode of this edge and end element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
            {
              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
            }
            if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626) // <= 50 degree)
            {
              setYawAllPosesOnEdge(posesOnEdge, false);
              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
            }
            else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276) // >= 130 degree
            {
              setYawAllPosesOnEdge(posesOnEdge, true);
              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
            }
            else
            {
              ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                       order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
              status = 3;
              message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                        ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
              return false;
              break;
            }
            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
            {
              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
              {
                if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
                {
                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
                }
                else
                {
                  ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                           order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
                  status = 3;
                  message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                            ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                  return false;
                  break;
                }
              }
              else
              {
                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
                status = 3;
                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                return false;
                break;
              }
            }
            else // posesOnPathWay is empty
            {
              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
            }
          }
          else
          {
            // if startNode of this edge and start element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
                orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
            {
              posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
            }
            // if endNode of this edge and end element of posesOnEdge are the different point
            if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
                orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
            {
              posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
            }
            // DeltaAngleStart <= 50 degree and DeltaAngleEnd <= 50 degree
            if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626 &&
                computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626)
            {
              setYawAllPosesOnEdge(posesOnEdge, false);
              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
              posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta);    // set yaw angle of the end pose to endNode theta
            }
            // DeltaAngleStart >= 130 degree and DeltaAngleEnd >= 130 degree
            else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276 &&
                     computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276)
            {
              setYawAllPosesOnEdge(posesOnEdge, true);
              posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
              posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta);    // set yaw angle of the end pose to endNode theta
            }
            if (!posesOnPathWay.empty()) // posesOnPathWay has datas
            {
              if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
                  posesOnEdge.front().getY() == posesOnPathWay.back().getY())
              {
                if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
                {
                  // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
                  posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
                  posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
                }
                else
                {
                  ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
                           order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
                  status = 3;
                  message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                            ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                  return false;
                  break;
                }
              }
              else
              {
                ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
                         order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
                status = 3;
                message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                          ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
                return false;
                break;
              }
            }
            else // posesOnPathWay is empty
            {
              posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
            }
          }
        }
        else
        {
          ROS_WARN("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", order.edges[i].edgeId.c_str(),
                   order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
          status = 2;
          message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
                    ", endNode: " + order.edges[i].endNodeId.c_str() + " is invalid NURBS-curve";
          return false;
          break;
        }
      }
      else
      {
        ROS_WARN("Edge: %s not found startNodeId: %s or endNodeId: %s", order.edges[i].edgeId.c_str(),
                 order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
        status = 1;
        message = "Edge: " + order.edges[i].edgeId + " not found startNodeId: " + order.edges[i].startNodeId.c_str() +
                  " or endNodeId: " + order.edges[i].endNodeId.c_str();
        return false;
        break;
      }
      // ROS_INFO("Finish to compute at Edge[%d]: %s", i, order.edges[i].edgeDescription.c_str());
    }

    status = 0;
    message = "Success to make plan: StartNode: " + order.edges[0].startNodeId + ", EndNode: " + order.edges[order.edges.size() - 1].endNodeId;

    if (input_spline_inf)
      delete (input_spline_inf);
    if (curve_design)
      delete (curve_design);
    return true;
  }

  bool isThetaValid(double theta)
  {
    bool result = false;
    if (theta < -M_PI || theta > M_PI)
      result = false;
    else
      result = true;
    return result;
  }

  bool curveIsValid(int degree, const std::vector<double> &knot_vector,
                    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &control_points)
  {
    if (degree < 1 || degree > 9)
    {
      ROS_WARN("degree is invalid value");
      return false;
    }
    if (!((knot_vector.size() - degree - 1) == control_points.size()))
    {
      ROS_WARN("relation between degree, number of knots, and number of control points is invalid");
      return false;
    }
    // if(std::is_sorted(knot_vector.begin(), knot_vector.end()))
    // {
    //   ROS_WARN("knot vector is not monotonic");
    //   return false;
    // }
    return true;
  }

  double computeDeltaAngleStartNode(double theta, vda_5050::Pose &startPose, vda_5050::Pose &next_Pose)
  {
    double delta_angle = 0;
    if (isThetaValid(theta))
    {
      double xAB = next_Pose.getX() - startPose.getX();
      double yAB = next_Pose.getY() - startPose.getY();
      double d = sqrt(xAB * xAB + yAB * yAB);
      double xC = startPose.getX() + d * cos(theta);
      double yC = startPose.getY() + d * sin(theta);
      double xAC = xC - startPose.getX();
      double yAC = yC - startPose.getY();
      double dAB = sqrt(xAB * xAB + yAB * yAB);
      double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
      if (cos_a > 1)
        cos_a = 1;
      else if (cos_a < (-1))
        cos_a = -1;
      delta_angle = acos(cos_a);
      // delta_angle = delta_angle*180/M_PI;
      // ROS_WARN("xC: %f, yC: %f", xC, yC);
      // ROS_WARN("dAB: %f", dAB);
      // ROS_WARN("delta_angle: %f", delta_angle);
    }
    return delta_angle;
  }

  double computeDeltaAngleEndNode(double theta, vda_5050::Pose &endPose, vda_5050::Pose &prev_Pose)
  {
    double delta_angle = 0;
    if (isThetaValid(theta))
    {
      double xAB = endPose.getX() - prev_Pose.getX();
      double yAB = endPose.getY() - prev_Pose.getY();
      double d = sqrt(xAB * xAB + yAB * yAB);
      double xC = endPose.getX() + d * cos(theta);
      double yC = endPose.getY() + d * sin(theta);
      double xBC = xC - endPose.getX();
      double yBC = yC - endPose.getY();
      double dAB = sqrt(xAB * xAB + yAB * yAB);
      double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
      if (cos_a > 1)
        cos_a = 1;
      else if (cos_a < (-1))
        cos_a = -1;
      delta_angle = acos(cos_a);
      // delta_angle = delta_angle*180/M_PI;
      // ROS_WARN("xC: %f, yC: %f", xC, yC);
      // ROS_WARN("dAB: %f", dAB);
      // ROS_WARN("delta_angle: %f", delta_angle);
    }
    return delta_angle;
  }

  double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose &endPose, geometry_msgs::Pose &prev_Pose)
  {
    double delta_angle = 0;
    if (isThetaValid(theta))
    {
      double xAB = endPose.position.x - prev_Pose.position.x;
      double yAB = endPose.position.y - prev_Pose.position.y;
      double d = sqrt(xAB * xAB + yAB * yAB);
      double xC = endPose.position.x + d * cos(theta);
      double yC = endPose.position.y + d * sin(theta);
      double xBC = xC - endPose.position.x;
      double yBC = yC - endPose.position.y;
      double dAB = sqrt(xAB * xAB + yAB * yAB);
      double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
      if (cos_a > 1)
        cos_a = 1;
      else if (cos_a < (-1))
        cos_a = -1;
      delta_angle = acos(cos_a);
      // delta_angle = delta_angle*180/M_PI;
      // ROS_WARN("xC: %f, yC: %f", xC, yC);
      // ROS_WARN("dAB: %f", dAB);
      // ROS_WARN("delta_angle: %f", delta_angle);
    }
    return delta_angle;
  }

  void setYawAllPosesOnEdge(std::vector<vda_5050::Pose> &posesOnEdge, bool reverse)
  {
    if (!reverse)
    {
      if (!posesOnEdge.empty())
      {
        if (posesOnEdge.size() > 2)
        {
          for (int i = 0; i < ((int)posesOnEdge.size() - 1); i++)
          {
            double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
                                          posesOnEdge[i + 1].getX(), posesOnEdge[i + 1].getY());
            posesOnEdge[i].setYaw(theta);
          }
          posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size() - 2].getYaw());
        }
        else if (posesOnEdge.size() == 2)
        {
          if (posesOnEdge[0].getX() != posesOnEdge[1].getX())
          {
            double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
                                          posesOnEdge[1].getX(), posesOnEdge[1].getY());
            posesOnEdge[0].setYaw(theta);
            posesOnEdge[1].setYaw(theta);
          }
        }
      }
    }
    else
    {
      if (!posesOnEdge.empty())
      {
        if (posesOnEdge.size() > 2)
        {
          for (int i = (int)posesOnEdge.size() - 1; i > 0; i--)
          {
            double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
                                          posesOnEdge[i - 1].getX(), posesOnEdge[i - 1].getY());
            posesOnEdge[i].setYaw(theta);
          }
          posesOnEdge.front().setYaw(posesOnEdge[1].getYaw());
        }
        else if (posesOnEdge.size() == 2)
        {
          if (posesOnEdge[1].getX() != posesOnEdge[0].getX())
          {
            double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
                                          posesOnEdge[0].getX(), posesOnEdge[0].getY());
            posesOnEdge[1].setYaw(theta);
            posesOnEdge[0].setYaw(theta);
          }
        }
      }
    }
  }

  double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose &Pose)
  {
    double delta_angle = 0;
    if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
    {
      double d = 1;
      double xA = Pose.getX();
      double yA = Pose.getY();
      double xB = xA + d * cos(thetaEnd);
      double yB = yA + d * sin(thetaEnd);
      double xAB = xB - xA;
      double yAB = yB - yA;
      double xC = xA + d * cos(thetaStart);
      double yC = yA + d * sin(thetaStart);
      double xAC = xC - xA;
      double yAC = yC - yA;
      double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
      if (cos_a > 1)
        cos_a = 1;
      else if (cos_a < (-1))
        cos_a = -1;
      delta_angle = acos(cos_a);
      // delta_angle = delta_angle*180/M_PI;
      // ROS_WARN("delta_angle: %f", delta_angle);
    }
    return delta_angle;
  }

  double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose &startPose, geometry_msgs::Pose &next_Pose)
  {
    double delta_angle = 0;
    if (isThetaValid(theta))
    {
      double xAB = next_Pose.position.x - startPose.position.x;
      double yAB = next_Pose.position.y - startPose.position.y;
      double d = sqrt(xAB * xAB + yAB * yAB);
      double xC = startPose.position.x + d * cos(theta);
      double yC = startPose.position.y + d * sin(theta);
      double xAC = xC - startPose.position.x;
      double yAC = yC - startPose.position.y;
      double dAB = sqrt(xAB * xAB + yAB * yAB);
      double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
      if (cos_a > 1)
        cos_a = 1;
      else if (cos_a < (-1))
        cos_a = -1;
      delta_angle = acos(cos_a);
      // delta_angle = delta_angle*180/M_PI;
      // ROS_WARN("xC: %f, yC: %f", xC, yC);
      // ROS_WARN("dAB: %f", dAB);
      // ROS_WARN("delta_angle: %f", delta_angle);
    }
    return delta_angle;
  }

  double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose &Pose)
  {
    double delta_angle = 0;
    if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
    {
      double d = 1;
      double xA = Pose.position.x;
      double yA = Pose.position.y;
      double xB = xA + d * cos(thetaEnd);
      double yB = yA + d * sin(thetaEnd);
      double xAB = xB - xA;
      double yAB = yB - yA;
      double xC = xA + d * cos(thetaStart);
      double yC = yA + d * sin(thetaStart);
      double xAC = xC - xA;
      double yAC = yC - yA;
      double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
      if (cos_a > 1)
        cos_a = 1;
      else if (cos_a < (-1))
        cos_a = -1;
      delta_angle = acos(cos_a);
      // delta_angle = delta_angle*180/M_PI;
      // ROS_WARN("delta_angle: %f", delta_angle);
    }
    return delta_angle;
  }

  double calculateDistance(vda_5050::Pose &pose1, vda_5050::Pose &pose2)
  {
    double dx = pose1.getX() - pose2.getX();
    double dy = pose1.getY() - pose2.getY();
    return std::sqrt(dx * dx + dy * dy);
  }

  double computeDeltaAngle(vda_5050::Pose &Pose1, vda_5050::Pose &Pose2)
  {
    double delta_angle = 0;

    double xA = Pose1.getX();
    double yA = Pose1.getY();
    double xB = Pose2.getX();
    double yB = Pose2.getY();
    double xAB = xB - xA;
    double yAB = yB - yA;
    double d = sqrt(xAB * xAB + yAB * yAB);
    double xC = xB + d * cos(Pose2.getYaw());
    double yC = yB + d * sin(Pose2.getYaw());
    double xBC = xC - xB;
    double yBC = yC - yB;
    double cos_a = (xAB * xBC + yAB * yBC) / (d * d);
    if (cos_a > 1)
      cos_a = 1;
    else if (cos_a < (-1))
      cos_a = -1;
    double delta_angle_tmp = acos(cos_a);
    if (delta_angle_tmp >= 1.5707963268)
    {
      delta_angle = M_PI - delta_angle_tmp;
    }
    else
    {
      delta_angle = delta_angle_tmp;
    }
    // delta_angle = delta_angle*180/M_PI;
    // ROS_WARN("delta_angle: %f", delta_angle);
    return delta_angle;
  }
}