#include <robot/robot.h>
#include <mkt_plugins/limited_accel_generator.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <robot_nav_core2/exceptions.h>
#include <vector>

namespace mkt_plugins
{

LimitedAccelGenerator::LimitedAccelGenerator()
  : StandardTrajectoryGenerator(),
    acceleration_time_(0.0)
{
}

LimitedAccelGenerator::~LimitedAccelGenerator()
{
}

void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
{
  StandardTrajectoryGenerator::initialize(nh);
  if (nh.hasParam("sim_period"))
  {
    nh.getParam("sim_period", acceleration_time_);
  }
  else
  {
    double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
    robot::log_warning("[%s:%d] limited_accel_generator: %f", __FILE__, __LINE__, controller_frequency);
    if (controller_frequency > 0)
    {
      acceleration_time_ = 1.0 / controller_frequency;
    }
    else
    {
      robot::log_warning_at(__FILE__, __LINE__, "A controller_frequency less than or equal to 0 has been set. "
                                              "Ignoring the parameter, assuming a rate of 20Hz");
      acceleration_time_ = 0.05;
    }
  }

}

void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity)
{
  // Limit our search space to just those within the limited acceleration_time
  velocity_iterator_->startNewIteration(current_velocity, acceleration_time_);
}

  score_algorithm::TrajectoryGenerator::Ptr LimitedAccelGenerator::create()
  {
    return std::make_shared<LimitedAccelGenerator>();
  }

}  // namespace mkt_plugins

BOOST_DLL_ALIAS(mkt_plugins::LimitedAccelGenerator::create, LimitedAccelGenerator)
