//
// Created by sebastian on 08.04.20.
//

#include "TrajectoryUtil.h"

const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";

bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
                                       moveit::planning_interface::MoveGroupInterface::Plan &plan,
                                       geometry_msgs::Pose targetPose,
                                       const std::string& pathType, double maxVelocityFactor,
                                       double maxAccelerationFactor, double motionSpeedFactor) {
  if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) {
    ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1].");
    return false;
  }
  if (maxAccelerationFactor <= 0 || maxAccelerationFactor > 1.0) {
    ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1].");
    return false;
  }
  if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
    ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
    return false;
  }

  group.setStartStateToCurrentState();
  group.setPlanningTime(10.0);
  group.setPoseTarget(targetPose);
  group.setMaxVelocityScalingFactor(maxVelocityFactor);
  group.setMaxAccelerationScalingFactor(maxAccelerationFactor);

  if (pathType == CARTESIAN_PATH) {
    std::vector<geometry_msgs::Pose> single_traj{targetPose};

    moveit_msgs::RobotTrajectory trajectory_msg;
    double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, true);
    ROS_INFO("Planned cartesian plan (%.2f%% achieved)", fraction * 100.0);

    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
    trajectory_processing::IterativeParabolicTimeParameterization iptp{};

    ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s");

    if (iptp.computeTimeStamps(rt)) {
      ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s");
    }

    if (iptp.computeTimeStamps(rt, maxVelocityFactor)) {
      ROS_INFO("Computed time stamps.");
      rt.getRobotTrajectoryMsg(trajectory_msg);
      plan.trajectory_ = trajectory_msg;

      ROS_ERROR_STREAM("After second time parameterization, the trajectory takes " << rt.getDuration() << "s");
    } else {
      ROS_ERROR("Computation of cartesian path has failed.");
      return false;
    }
  } else if (pathType == FLUID_PATH) {
    if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
      ROS_ERROR("Computation of fluid path has failed.");
    }
  } else {
    ROS_ERROR("Invalid type of path provided.");
    return false;
  }

  manipulateVelocity(plan, motionSpeedFactor);
}

bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
                                   geometry_msgs::Pose &initialPose) {
  std::vector<double> x_values;
  std::vector<double> y_values;
  std::vector<double> z_values;

  // check if no trajectory is configured
  if (!node_handle.getParam("trajectory_pos_x", x_values) ||
      !node_handle.getParam("trajectory_pos_y", y_values) ||
      !node_handle.getParam("trajectory_pos_z", z_values)) {
    return false;
  } else {
    for (int i = 0; i < x_values.size(); i++) {
      geometry_msgs::Pose pose = initialPose;
      pose.position.x = x_values.at(i);
      pose.position.y = y_values.at(i);
      pose.position.z = z_values.at(i);
      waypoints.push_back(pose);
    }

    node_handle.setParam("new_trajectory_available", false);
    return true;
  }
}

void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) {
  ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << "joints");
  for (auto &point : plan.trajectory_.joint_trajectory.points) {
    point.time_from_start *= 1 / velocity;
    for (double &v : point.velocities) {
      v *= velocity;
    }

    for (double &acceleration : point.accelerations) {
      acceleration = sqrt(std::abs(acceleration));
    }
  }

}