#include <robot/robot.h>
#include <mkt_plugins/goal_checker.h>
#include <boost/dll/alias.hpp>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <angles/angles.h>
#include <cmath>

mkt_plugins::GoalChecker::GoalChecker()
  : nh_(),
    line_generator_(nullptr),
    yaw_goal_tolerance_(0.25),
    xy_goal_tolerance_(0.25),
    old_xy_goal_tolerance_(0.0)
{
}

void mkt_plugins::GoalChecker::initialize(const robot::NodeHandle &nh)
{
  nh_ = nh;
  this->intParam(nh);
  robot::log_info_at(__FILE__, __LINE__, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f", xy_goal_tolerance_, yaw_goal_tolerance_);
  line_generator_ = std::make_shared<mkt_plugins::LineGenerator>();
}

void mkt_plugins::GoalChecker::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);
  }
}

void mkt_plugins::GoalChecker::reset()
{
  old_xy_goal_tolerance_ = 0;
}

bool mkt_plugins::GoalChecker::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_);
  double dx = query_pose.pose.x - goal_pose.pose.x;
  double dy = query_pose.pose.y - goal_pose.pose.y;
  double xy_tolerance = std::abs(std::sqrt(dx * dx + dy * dy));

  if (!path.poses.empty() && path.poses.size() > 2)
  {
    double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
    for(int i = path.poses.size() - 1; i >= 0; i--)
    {
      double dx = path.poses[i].pose.x - path.poses.back().pose .x;
      double dy = path.poses[i].pose.y - path.poses.back().pose.y;
      if(std::sqrt(dx * dx + dy * dy) > 0.05 // TODO: get resolution from costmap
         && fabs(dx) > 1e-6 && fabs(dy) > 1e-6)
      {
        theta =  angles::normalize_angle(query_pose.pose.theta - atan2(dy, dx));
        break;
      }
    }
    double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
    double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
    if (xy_tolerance <= xy_goal_tolerance_ + 0.1)
    {
      double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
      if(fabs(tolerance) <= xy_goal_tolerance_) 
      {
        robot::log_info_at(__FILE__, __LINE__, "%.3f %.3f %.3f %.3f %.3f", fabs(cos(theta)), fabs(sin(theta)),xy_tolerance, xy_goal_tolerance_, yaw_goal_tolerance_);
        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
        return true;
      }
    }
    else
      old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
  }
  else
  {
    double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta);
    double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy;
    double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy;
    if (xy_tolerance <= xy_goal_tolerance_ + 0.1)
    {
      double tolerance = fabs(x) > fabs(y) ? x : y;
      if(
          (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) 
        )
      {
        robot::log_info_at(__FILE__, __LINE__, "%f %f", fabs(cos(theta)), fabs(sin(theta)));
        robot::log_info_at(__FILE__, __LINE__, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
        return true;
      }
    }
    else
      old_xy_goal_tolerance_ = fabs(x) > fabs(y) ? x : y;
  }

  if (xy_tolerance <= xy_goal_tolerance_)
  {
    robot::log_info_at(__FILE__, __LINE__, "Goal checker ok %f %f", xy_tolerance, xy_goal_tolerance_);
    return true;
  }
  return false;
}

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

BOOST_DLL_ALIAS(mkt_plugins::GoalChecker::create, GoalChecker)