#include <mkt_plugins/standard_traj_generator.h>
#include <mkt_plugins/xy_theta_iterator.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <robot_nav_core2/exceptions.h>
#include <algorithm>
#include <cmath>
#include <memory>
#include <string>
#include <vector>

using robot_nav_2d_utils::loadParameterWithDeprecation;

namespace mkt_plugins
{

  StandardTrajectoryGenerator::StandardTrajectoryGenerator()
    : nh_kinematics_(),
      kinematics_(nullptr),
      velocity_iterator_(nullptr),
      sim_time_(0.0),
      discretize_by_time_(false),
      time_granularity_(0.0),
      linear_granularity_(0.0),
      angular_granularity_(0.0),
      include_last_point_(false)
  {
  }

  StandardTrajectoryGenerator::~StandardTrajectoryGenerator()
  {
  }

  void StandardTrajectoryGenerator::initialize(const robot::NodeHandle &nh)
  {
    nh_kinematics_ = nh;
    kinematics_ = std::make_shared<mkt_plugins::KinematicParameters>();
    kinematics_->initialize(nh);
    initializeIterator(nh);

    nh.param("sim_time", sim_time_, 1.7);

    nh.param("include_last_point", include_last_point_, true);
    /*
     * If discretize_by_time, then sim_granularity represents the amount of time that should be between
     *  two successive points on the trajectory.
     *
     * If discretize_by_time is false, then sim_granularity is the maximum amount of distance between
     *  two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
     *  angular distance between two successive points.
     */
    nh.param("discretize_by_time", discretize_by_time_, false);
    if (discretize_by_time_)
    {
      time_granularity_ = loadParameterWithDeprecation(nh, "time_granularity", "sim_granularity", 0.025);
    }
    else
    {
      linear_granularity_ = loadParameterWithDeprecation(nh, "linear_granularity", "sim_granularity", 0.025);
      angular_granularity_ = loadParameterWithDeprecation(nh, "angular_granularity", "angular_sim_granularity", 0.1);
    }
  }

  robot::NodeHandle StandardTrajectoryGenerator::getNodeHandle()
  {
    return nh_kinematics_;
  }

  void StandardTrajectoryGenerator::initializeIterator(const robot::NodeHandle &nh)
  {
    velocity_iterator_ = std::make_shared<mkt_plugins::XYThetaIterator>();
    velocity_iterator_->initialize(nh, kinematics_);
  }

  void StandardTrajectoryGenerator::setDirect(int *direct)
  {
    kinematics_->setDirect(direct);
  }

  bool StandardTrajectoryGenerator::setTwistLinear(robot_geometry_msgs::Vector3 linear)
  {
    try
    {
      // ROS_INFO("Get linear %f ", linear.x);
      if (linear.x > 0)
        kinematics_->setMaxX(linear.x);
      else if (linear.x < 0)
        kinematics_->setMinX(linear.x);
      else
      {
        kinematics_->setMinX(linear.x);
        kinematics_->setMaxX(linear.x);
      }

      if (linear.y > 0)
        kinematics_->setMaxY(linear.y);
      else if (linear.y < 0)
        kinematics_->setMinY(linear.y);
      else
      {
        kinematics_->setMinY(linear.y);
        kinematics_->setMaxY(linear.y);
      }
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
      return false;
    }

    return true;
  }

  robot_geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct)
  {
    robot_geometry_msgs::Vector3 linear;
    try
    {
      if (direct)
      {
        linear.x = kinematics_->getMaxX();
        linear.y = kinematics_->getMaxY();
      }
      else
      {
        linear.x = kinematics_->getMinX();
        linear.y = kinematics_->getMinY();
      }
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
    }
    return linear;
  }

  bool StandardTrajectoryGenerator::setTwistAngular(robot_geometry_msgs::Vector3 angular)
  {
    try
    {
      kinematics_->setMaxTheta(angular.z);
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
      return false;
    }
    return true;
  }

  robot_geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct)
  {
    robot_geometry_msgs::Vector3 angular;
    try
    {
      if (direct)
      {
        angular.z = kinematics_->getMaxTheta();
      }
      else
      {
        angular.z = kinematics_->getMinTheta();
      }
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
    }
    return angular;
  }

  void StandardTrajectoryGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D &current_velocity)
  {
    velocity_iterator_->startNewIteration(current_velocity, sim_time_);
  }

  bool StandardTrajectoryGenerator::hasMoreTwists()
  {
    return velocity_iterator_->hasMoreTwists();
  }

  robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist()
  {
    return velocity_iterator_->nextTwist();
  }

  robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::getTwist()
  {
    return velocity_iterator_->getTwist();
  }

  std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel)
  {
    std::vector<double> steps;
    if (discretize_by_time_)
    {
      steps.resize(ceil(sim_time_ / time_granularity_));
    }
    else // discretize by distance
    {
      double vmag = hypot(cmd_vel.x, cmd_vel.y);

      // the distance the robot would travel in sim_time if it did not change velocity
      double projected_linear_distance = vmag * sim_time_;

      // the angle the robot would rotate in sim_time
      double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;

      // Pick the maximum of the two
      int num_steps = ceil(std::max(projected_linear_distance / linear_granularity_,
                                    projected_angular_distance / angular_granularity_));
      steps.resize(num_steps);
    }
    if (steps.size() == 0)
    {
      steps.resize(1);
    }
    std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
    return steps;
  }

  mkt_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const robot_nav_2d_msgs::Pose2DStamped &start_pose,
                                                                         const robot_nav_2d_msgs::Path2D &transformed_plan,
                                                                         const robot_nav_2d_msgs::Twist2D &start_vel,
                                                                         const robot_nav_2d_msgs::Twist2D &cmd_vel)
  {
    mkt_msgs::Trajectory2D traj;
    traj.velocity = cmd_vel;

    //  simulate the trajectory
    robot_geometry_msgs::Pose2D pose = start_pose.pose;
    robot_nav_2d_msgs::Twist2D vel = start_vel;
    double running_time = 0.0;
    std::vector<double> steps = getTimeSteps(cmd_vel);

    for (double dt : steps)
    {
      traj.poses.push_back(pose);
      traj.time_offsets.push_back(robot::Duration(running_time));
      //  calculate velocities
      vel = computeNewVelocity(cmd_vel, vel, dt);

      //  update the position of the robot using the velocities passed in
      pose = computeNewPosition(pose, vel, dt);
      running_time += dt;
    } //  end for simulation steps

    if (include_last_point_)
    {
      traj.poses.push_back(pose);
      traj.time_offsets.push_back(robot::Duration(running_time));
    }

    return traj;
  }

  /**
   * change vel using acceleration limits to converge towards sample_target-vel
   */
  robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const robot_nav_2d_msgs::Twist2D &cmd_vel,
                                                                       const robot_nav_2d_msgs::Twist2D &start_vel, const double dt)
  {
    robot_nav_2d_msgs::Twist2D new_vel;
    new_vel.x = projectVelocity(start_vel.x, kinematics_->getAccX(), kinematics_->getDecelX(), dt, cmd_vel.x);
    new_vel.y = projectVelocity(start_vel.y, kinematics_->getAccY(), kinematics_->getDecelY(), dt, cmd_vel.y);
    new_vel.theta = projectVelocity(start_vel.theta, kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
                                    dt, cmd_vel.theta);
    return new_vel;
  }

  robot_geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const robot_geometry_msgs::Pose2D start_pose,
                                                                        const robot_nav_2d_msgs::Twist2D &vel, const double dt)
  {
    robot_geometry_msgs::Pose2D new_pose;
    new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
    new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
    new_pose.theta = start_pose.theta + vel.theta * dt;
    return new_pose;
  }

  score_algorithm::TrajectoryGenerator::Ptr StandardTrajectoryGenerator::create()
  {
    return std::make_shared<StandardTrajectoryGenerator>();
  }
} // namespace mkt_plugins

BOOST_DLL_ALIAS(mkt_plugins::StandardTrajectoryGenerator::create, StandardTrajectoryGenerator)
