#ifndef _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
#define _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
#include <ros/ros.h>
#include "amr_comunication/vda_5050/utils/curve_common.h"
#include "amr_comunication/vda_5050/utils/pose.h"
#include <amr_comunication/vda_5050/utils/common.h>


namespace amr_control
{
  inline double getYawDegree(double x, double y, double z, double w) {
    // yaw (z-axis rotation)
    double siny_cosp = 2 * (w * z + x * y);
    double cosy_cosp = 1 - 2 * (y * y + z * z);
    double yaw = std::atan2(siny_cosp, cosy_cosp);
    yaw = (180 * yaw) / M_PI;return yaw;
  }

  inline double getYawRad(double x, double y, double z, double w) {
    // yaw (z-axis rotation)
    double siny_cosp = 2 * (w * z + x * y);
    double cosy_cosp = 1 - 2 * (y * y + z * z);
    double yaw = std::atan2(siny_cosp, cosy_cosp);
    return yaw;
  }

  inline double calculateAngle(double xA, double yA, double xB, double yB) {
    double deltaX = xB - xA;
    double deltaY = yB - yA;
    double angleRad = atan2(deltaY, deltaX);
    // double angleDeg = angleRad * 180.0 / M_PI;
    return angleRad;
  }

  inline void modifyYaw(double& yaw) {
    while (yaw < -M_PI)
      yaw += 2.0 * M_PI;
    while (yaw > M_PI)
      yaw -= 2.0 * M_PI;
  };

  double calculateDistance(vda_5050::Pose& pose1, vda_5050::Pose& pose2);

  double computeDeltaAngle(vda_5050::Pose& Pose1, vda_5050::Pose& Pose2);

  bool isThetaValid(double theta);

  bool curveIsValid(int degree, const std::vector<double>& knot_vector, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);

  double computeDeltaAngleStartNode(double theta, vda_5050::Pose& startPose, vda_5050::Pose& next_Pose);

  double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose& Pose);

  double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose);

  double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose& Pose);

  double computeDeltaAngleEndNode(double theta, vda_5050::Pose& endPose, vda_5050::Pose& prev_Pose);

  double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose);

  void setYawAllPosesOnEdge(std::vector<vda_5050::Pose>& posesOnEdge, bool reverse);

  bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t& status, std::string& message, std::vector<vda_5050::Pose>& posesOnPathWay);
}

#endif // _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_