#include <mkt_plugins/simple_goal_checker.h>
#include <boost/dll/alias.hpp>
#include <angles/angles.h>
#include <robot_nav_2d_utils/parameters.h>
#include <cmath>

namespace mkt_plugins
{

SimpleGoalChecker::SimpleGoalChecker()
  : nh_(),
    xy_goal_tolerance_(0.25),
    yaw_goal_tolerance_(0.25),
    stateful_(true),
    check_xy_(true),
    xy_goal_tolerance_sq_(0.0625)
{
}

void SimpleGoalChecker::initialize(const robot::NodeHandle& nh)
{
  nh_ = nh;
  this->intParam(nh);
  robot::log_info_at(__FILE__, __LINE__, "[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_);
}

void SimpleGoalChecker::intParam(const robot::NodeHandle& nh)
{
  if(!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
  {
    xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
  }
  if(!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
  {
    yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
  }
  if(nh.param<bool>("stateful", stateful_, true))
  {
    stateful_ = robot_nav_2d_utils::searchAndGetParam(nh, "stateful", true);
  }
  robot::log_info_throttle(1.0,"[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_);
  xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
}

void SimpleGoalChecker::reset()
{
  check_xy_ = true;
}

bool SimpleGoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& query_pose, const robot_nav_2d_msgs::Pose2DStamped& goal_pose,
                                      const robot_nav_2d_msgs::Path2D& path, const robot_nav_2d_msgs::Twist2D& velocity)
{
  this->intParam(nh_);
  if (check_xy_)
  {
    double dx = query_pose.pose.x - goal_pose.pose.x,
           dy = query_pose.pose.y - goal_pose.pose.y;
    if (dx * dx + dy * dy > 0.25)
    {
      return false;
    }
    // We are within the window
    // If we are stateful, change the state.
    if (stateful_)
    {
      check_xy_ = false;
    }
  }
  double dyaw = angles::shortest_angular_distance(query_pose.pose.theta, goal_pose.pose.theta);
  // robot::log_info_throttle(1.0, "dyaw %f yaw_goal_tolerance_ %f", fabs(dyaw), yaw_goal_tolerance_);
  return fabs(dyaw) < yaw_goal_tolerance_;
}

}  // namespace mkt_plugins

score_algorithm::GoalChecker::Ptr mkt_plugins::SimpleGoalChecker::create()
{
  return std::make_shared<mkt_plugins::SimpleGoalChecker>();
}

BOOST_DLL_ALIAS(mkt_plugins::SimpleGoalChecker::create, SimpleGoalChecker)
