#include <ros/ros.h>
#include <pluginlib/class_loader.h>
#include <memory>
#include <string>
#include <move_base_core/navigation.h>
#include <amr_control/moveTo.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int16.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <vda5050_msgs/Order.h>
#include <vda5050_msgs/InstantActions.h>

#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/Pose2D.h>

move_base_core::BaseNavigation::Ptr move_base = nullptr;
geometry_msgs::PoseStamped amcl_pose;
boost::mutex ctr_mutex;
bool pub_order = false;
amr_control::moveTo order;
bool pub_cancel = false;

void moveToCallback(const amr_control::moveTo::ConstPtr& msg)
{
  // boost::unique_lock<boost::mutex> lock(ctr_mutex);
  if(move_base)
  {
    try
    {
      geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(msg->goal, "map", ros::Time::now());
      geometry_msgs::PoseStamped of_goal =  move_base_core::offset_goal(amcl_pose, msg->goal.x);
      double xy_goal_tolerance = 0.04;
      double yaw_goal_tolerance = 0.06;
      // ROS_INFO_STREAM(of_goal);
      switch ((int)msg->maker.data)
      {
      case 1:
        move_base->moveTo(goal, move_base_core::PICK_UP, xy_goal_tolerance, yaw_goal_tolerance);
        break;
      case 2:
        move_base->moveTo(goal, move_base_core::POP_DOWN, xy_goal_tolerance, yaw_goal_tolerance);
        break; 
      case 3:
        move_base->moveTo(goal, move_base_core::POSITION, xy_goal_tolerance, yaw_goal_tolerance);
        break; 
      case 4:
        move_base->moveTo(goal, move_base_core::CHARGER, xy_goal_tolerance, yaw_goal_tolerance);
        break; 
      case 5:
        move_base->moveStraightTo(of_goal, xy_goal_tolerance);
        break; 
      case 6:
        move_base->rotateTo(goal, yaw_goal_tolerance);
        break; 
      default:
          throw std::bad_function_call();
        break;
      }

      geometry_msgs::Vector3 linear;
      linear.x = 1.1;
      move_base->setTwistLinear(linear);
      linear.x = -0.5;
      move_base->setTwistLinear(linear);
    }
    catch(const std::exception& e)
    {
      ROS_ERROR_STREAM(e.what() << "\n");
    }
  }
}

void getposeCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
  amcl_pose.header = msg->header;
  amcl_pose.pose.position = msg->pose.pose.position;
  amcl_pose.pose.orientation = msg->pose.pose.orientation;
}

void setTiwstCallback(const geometry_msgs::Vector3::ConstPtr& msg)
{
  if(move_base)
  {
    move_base->setTwistLinear(*msg);
  }
}

void cancelCallback(const std_msgs::Bool::ConstPtr& msg)
{
  move_base->cancel();
}

void pauseCallback(const std_msgs::Bool::ConstPtr& msg)
{
  move_base->pause();
}

void resumeCallback(const std_msgs::Bool::ConstPtr& msg)
{
  move_base->resume();
}

void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg)
{
  if(msg->serialNumber == "AMR-150")
  {
    ros::Duration(0.5).sleep();
    if(!msg->nodes.empty())
    {
      order.header.frame_id = "map";
      order.header.stamp = ros::Time::now();
      order.goal.x = msg->nodes.back().nodePosition.x;
      order.goal.y = msg->nodes.back().nodePosition.y;
      order.goal.theta = msg->nodes.back().nodePosition.theta;
      if(!msg->nodes.back().actions.empty())
      {
        if(msg->nodes.back().actions.front().actionType == "startInPallet")
          order.maker.data = 1;
        else
          order.maker.data = 3;
      }
      else 
        order.maker.data = 3;
      pub_order = true;
    }
  }
}

void instantActions_msg_handle(const vda5050_msgs::InstantActions::ConstPtr& msg)
{
  if(msg->serialNumber == "AMR-150")
  {
    for(int i=0; i< msg->actions.size(); i++)
    {
      if(msg->actions[i].actionType == "cancelOrder") pub_cancel = true;
    }
  }
}

bool getRobotPose(geometry_msgs::PoseStamped& global_pose, std::shared_ptr<tf2_ros::Buffer> tf_)
{
	std::string global_frame_ = "map";
	std::string robot_base_frame_ = "base_link";
  // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose");
  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
  geometry_msgs::PoseStamped robot_pose;
  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
  robot_pose.header.frame_id = robot_base_frame_;
  robot_pose.header.stamp = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

  // get the global pose of the robot
  try
  {
    // use current time if possible (makes sure it's not in the future)
    if (tf_->canTransform(global_frame_, robot_base_frame_, current_time))
    {
      geometry_msgs::TransformStamped transform = tf_->lookupTransform(global_frame_, robot_base_frame_, current_time);
      tf2::doTransform(robot_pose, global_pose, transform);
    }
    // use the latest otherwise
    else
    {
      tf_->transform(robot_pose, global_pose, global_frame_);
    }
  }
  catch (tf2::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf2::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf2::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
  // check global_pose timeout
  if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > 1.0)
  {
    ROS_WARN_THROTTLE(1.0,
    // ROS_WARN(
                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), global_pose.header.stamp.toSec(), 1.0);
    return false;
  }

  return true;
}


int main(int argc, char** argv){
  ros::init(argc, argv, "move_base_node");
  ros::NodeHandle nh = ros::NodeHandle("move_base_node");
  ros::NodeHandle nh_;
  std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
  tf2_ros::TransformListener tf2(*buffer);

  pluginlib::ClassLoader<move_base_core::BaseNavigation> mb_loader("move_base_core", "move_base_core::BaseNavigation");
  
  std::string obj_name("move_base::MoveBase");
  try
  {
    move_base = mb_loader.createUniqueInstance(obj_name);
    ROS_INFO("Created object %s susseced", obj_name.c_str());
    move_base->initialize(buffer);
  }
  catch (pluginlib::PluginlibException &ex)
  {
    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
    return 1;
  }
  catch(std::exception &e)
  {
    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
    return 1;
  }
  ros::Subscriber moveto_sub = nh.subscribe("moveTo", 1000, moveToCallback);
  ros::Subscriber set_twist_sub = nh.subscribe("setTiwst", 1000, setTiwstCallback);
  ros::Subscriber cancel_sub = nh.subscribe("cancel", 1000, cancelCallback);
  ros::Subscriber pause_sub = nh.subscribe("pause", 1000, pauseCallback);
  ros::Subscriber resume_sub = nh.subscribe("resume", 1000, resumeCallback);
  ros::Subscriber amcl_pose_sub = nh.subscribe("amcl_pose", 1000, getposeCallback);
  ros::Subscriber order_msg_sub_ = nh_.subscribe("/order",1000, order_msg_handle);
  ros::Subscriber instantAc_msg_sub_ = nh_.subscribe("/instantActions",1000, instantActions_msg_handle);
  ros::Publisher  moveto_pub = nh.advertise<amr_control::moveTo>("moveTo", 1000);
  ros::Publisher  cancel_pub = nh.advertise<std_msgs::Bool>("cancel", 1000);
  ros::Rate r(10);
  
  while (ros::ok())
  {
    if(pub_order)
    {
      moveto_pub.publish(order);
      pub_order = false;
    }
    else if (pub_cancel)
    {
      std_msgs::Bool value;
      value.data = true;
      cancel_pub.publish(value);
      pub_cancel = false;
    }
    getRobotPose(amcl_pose, buffer);
    // boost::unique_lock<boost::mutex> lock(ctr_mutex);
    // {
      if(move_base && move_base->nav_feedback_)
      {
        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::ABORTED)
        //   ROS_ERROR("ABORTED %s", move_base->nav_feedback_->feed_back_str.c_str());
        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::PAUSED) 
        //   ROS_WARN("PAUSED %s", move_base->nav_feedback_->feed_back_str.c_str());
        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::PREEMPTED) 
        //   ROS_WARN("PREEMPTED %s", move_base->nav_feedback_->feed_back_str.c_str());
        // if(move_base->nav_feedback_->navigation_state ==  move_base_core::State::SUCCEEDED) 
        //   ROS_INFO_STREAM("SUCCEEDED \n" << move_base->nav_feedback_->current_pose);
      }
    // }
    ros::spinOnce();
    r.sleep();
  }
  
  ros::spin();
  return(0);
}