/**
 * \file  gazebo_ros_steer_drive.cpp
 * \brief A fork lifter plugin for gazebo - taken from the old SAUNA repo.
 * \author  Henrik Andreasson <henrik.andreasson@oru.se>
 */


#include <algorithm>
#include <assert.h>

#include <ros_lift_controller/ros_jack_lifter.h>

 //#include <gazebo/math/gzmath.hh>
#include <sdf/sdf.hh>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/Odometry.h>
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
#include <ros_lift_controller/Lift_Measurement.h>

namespace gazebo
{
  RosLiftLifter::RosLiftLifter() {}

  // Destructor
  RosLiftLifter::~RosLiftLifter() {}

  // Load the controller
  void RosLiftLifter::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
  {
    parent = _parent;
    gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "ForkLifter"));
    // Make sure the ROS node for Gazebo has already been initialized
    gazebo_ros_->isInitialized();

    gazebo_ros_->getParameter<double>(update_rate_, "updateRate", 100.0);

    gazebo_ros_->getParameter<double>(lift_torque_, "liftTorque", 1000.0);
    gazebo_ros_->getParameter<double>(rotate_torque_, "rotateTorque", 1000.0);
    gazebo_ros_->getParameter<double>(lift_height_maximum_, "liftHeightMaximum", 0.1);

    gazebo_ros_->getParameter<std::string>(robot_base_frame_, "robotBaseFrame", "base_link");

    // Gazebo specific height to align the forks to the origin.
    gazebo_ros_->getParameter<double>(lift_height_origin_, "liftHeightOrigin", 0.08);

    gazebo_ros_->getParameterBoolean(publishTF_, "publishTF", false);
    gazebo_ros_->getParameterBoolean(publishJointState_, "publishJointState", false);

    gazebo_ros_->getParameter<std::string>(command_topic_, "commandTopic", "cmd_jacking");
    // gazebo_ros_->getParameterBoolean(use_velocity_control_, "useVelocityControl", true);
    // gazebo_ros_->getParameter<double>(max_velocity_, "maxVelocity", 0.08);

    gazebo_ros_->getParameter<double>(lift_step_, "liftStep", 0.002);
    gazebo_ros_->getParameter<double>(rotate_step_, "rotateStep", 0.05);

    double pid_p, pid_i, pid_d;
    gazebo_ros_->getParameter<double>(pid_p, "pidP", 2000);
    gazebo_ros_->getParameter<double>(pid_i, "pidI", 0);
    gazebo_ros_->getParameter<double>(pid_d, "pidD", 10);

    base_lifting_joint_ = gazebo_ros_->getJoint(parent, "liftJoint", "base_lifting_joint");
    base_rotating_joint_ = gazebo_ros_->getJoint(parent, "rotateJoint", "base_rotating_joint");
#if GAZEBO_MAJOR_VERSION > 2
    base_lifting_joint_->SetParam("fmax", 0, lift_torque_);
    base_rotating_joint_->SetParam("fmax", 0, rotate_torque_);
#else
    base_lifting_joint_->SetMaxForce(0, lift_torque_);
    base_rotating_joint_->SetMaxForce(0, rotate_torque_);
#endif

    // Initialize update rate stuff
    if (this->update_rate_ > 0.0) this->update_period_ = 1.0 / this->update_rate_;
    else this->update_period_ = 0.0;

#if GAZEBO_MAJOR_VERSION >= 8
    last_actuator_update_ = parent->GetWorld()->SimTime();
#else
    last_actuator_update_ = parent->GetWorld()->GetSimTime();
#endif

    // Initialize velocity stuff
    alive_ = true;
    this->joint_lift_pid_.Init(pid_p, pid_i, pid_d, 100, -100, lift_torque_, -lift_torque_);
    this->joint_rotate_pid_.Init(pid_p, pid_i, pid_d, 100, -100, rotate_torque_, -rotate_torque_);

    if (this->publishJointState_) {
      joint_state_publisher_ = gazebo_ros_->node()->advertise<sensor_msgs::JointState>("joint_states", 1000);
      ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info());
    }

    transform_broadcaster_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());

    // ROS: Subscribe to the velocity command topic (cmd_fork)
    ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str());

    ros::SubscribeOptions so =
      ros::SubscribeOptions::create<geometry_msgs::Pose>(command_topic_, 1,
        boost::bind(&RosLiftLifter::cmdCallback, this, _1),
        ros::VoidPtr(), &queue_);

    cmd_lift_subscriber_ = gazebo_ros_->node()->subscribe(so);
    ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str());
    state_lift_publisher_ = gazebo_ros_->node()->advertise<ros_lift_controller::Lift_Measurement>("lift_sensor_states", 100);

    // start custom queue for diff drive
    this->callback_queue_thread_ = boost::thread(boost::bind(&RosLiftLifter::QueueThread, this));

    // listen to the update event (broadcast every simulation iteration)
    this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&RosLiftLifter::UpdateChild, this));

    lift_height_cmd_ = 0.0;
    current_lift_position_ = 0.0;
    lift_rotate_cmd_= 0.0;
    current_lift_angle_ = 0.0;
  }

  void RosLiftLifter::publishLiftSensorStates()
  {
    ros_lift_controller::Lift_Measurement lm;
    lm.header.stamp = ros::Time::now();
    lm.header.frame_id = robot_base_frame_;

#if GAZEBO_MAJOR_VERSION >= 8
      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose();
#else
      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign();
#endif

    lm.angle_position = pose_rotate.Rot().Euler().Z();

    lm.top_sensor = fabs(current_lift_position_ - lift_height_maximum_) < 0.01? true : false;
    lm.bottom_sensor = fabs(current_lift_position_ - 0.0) < 0.01? true : false;
    state_lift_publisher_.publish(lm);
  }

  void RosLiftLifter::publishJointState(physics::JointPtr joint)
  {
    std::vector<physics::JointPtr> joints;
    joints.push_back(joint);

    ros::Time current_time = ros::Time::now();
    sensor_msgs::JointState joint_state;
    joint_state.header.stamp = current_time;
    joint_state.name.resize(joints.size());
    joint_state.position.resize(joints.size());
    joint_state.velocity.resize(joints.size());
    joint_state.effort.resize(joints.size());
    for (std::size_t i = 0; i < joints.size(); i++) {
      joint_state.name[i] = joints[i]->GetName();
#if GAZEBO_MAJOR_VERSION >= 8
      joint_state.position[i] = joints[i]->Position(0);
#else
      joint_state.position[i] = joints[i]->GetAngle(0).Radian();
#endif
      joint_state.velocity[i] = joints[i]->GetVelocity(0);
      joint_state.effort[i] = joints[i]->GetForce(0);
    }
    joint_state_publisher_.publish(joint_state);
  }

  void RosLiftLifter::publishTF(physics::JointPtr joint)
  {
    ros::Time current_time = ros::Time::now();
    std::vector<physics::JointPtr> joints;
    joints.push_back(joint);

    for (std::size_t i = 0; i < joints.size(); i++) {
      std::string frame = gazebo_ros_->resolveTF(joints[i]->GetName());
      std::string parent_frame = gazebo_ros_->resolveTF(joints[i]->GetParent()->GetName());

#if GAZEBO_MAJOR_VERSION >= 8
      ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose();
#else
      ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign();
#endif

      tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
      tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());

      tf::Transform transform(qt, vt);
      transform_broadcaster_->sendTransform(tf::StampedTransform(transform, current_time, parent_frame, frame));
    }

  }
  // Update the controller
  void RosLiftLifter::UpdateChild()
  {
    UpdateForkEncoder();
#if GAZEBO_MAJOR_VERSION >= 8
    common::Time current_time = parent->GetWorld()->SimTime();
#else
    common::Time current_time = parent->GetWorld()->GetSimTime();
#endif

    double seconds_since_last_update = (current_time - last_actuator_update_).Double();
    if (seconds_since_last_update > update_period_) {

      if (publishJointState_) 
      {
        publishJointState(base_lifting_joint_);
        publishJointState(base_rotating_joint_);
      }
      publishLiftSensorStates();

// Use the PID class...
#if GAZEBO_MAJOR_VERSION >= 8
      ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->RelativePose();
#else
      ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->GetRelativePose().Ign();
#endif
      double target_lift_height = lift_height_cmd_ + this->lift_height_origin_;
      double current_lift_height = pose_lift.Pos().Z();
      double error_lift_height = current_lift_height - target_lift_height;
      this->effortController(base_lifting_joint_, error_lift_height, seconds_since_last_update);
      this->positionController(base_lifting_joint_, lift_height_cmd_, current_lift_position_, lift_step_);

#if GAZEBO_MAJOR_VERSION >= 8
      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose();
#else
      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign();
#endif

      double target_lift_yaw = lift_rotate_cmd_;
      double current_lift_yaw = pose_rotate.Rot().Euler().Z();
      double error_lift_yaw = current_lift_yaw - target_lift_yaw;
      this->effortController(base_rotating_joint_, error_lift_yaw, seconds_since_last_update);
      this->positionController(base_rotating_joint_, lift_rotate_cmd_, current_lift_angle_, rotate_step_);

      last_actuator_update_ += common::Time(update_period_);
    }

#if 0
    if (odom_source_ == ENCODER) UpdateOdometryEncoder();
    common::Time current_time = parent->GetWorld()->GetSimTime();
    double seconds_since_last_update = (current_time - last_actuator_update_).Double();
    if (seconds_since_last_update > update_period_) {

      publishOdometry(seconds_since_last_update);
      if (publishWheelTF_) publishWheelTF();
      if (publishWheelJointState_) publishWheelJointState();

      double target_wheel_roation_speed = cmd_.speed / (wheel_diameter_ / 2.0);
      double target_steering_angle_speed = cmd_.angle;

      motorController(target_wheel_roation_speed, target_steering_angle_speed, seconds_since_last_update);

      //ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle);

      last_actuator_update_ += common::Time(update_period_);
    }
#endif
  }

  void RosLiftLifter::effortController(physics::JointPtr joint, const double &error, const double &dt)
  {
    double control_value = this->joint_lift_pid_.Update(error, dt);
#if GAZEBO_MAJOR_VERSION > 2
    joint->SetForce(0, control_value);
#else
    joint->SetForce(0, control_value);
#endif
  }

  void RosLiftLifter::positionController(physics::JointPtr joint, const double &target, double &current , const double &dt)
  {
    double error = target - current;
    double step = dt;
    // Nếu sai số lớn, tăng/giảm dần
    if (std::abs(error) > step)
    {
      current += step * (error > 0 ? 1 : -1);
    }
    else
    {
      current = target;  // gần bằng rồi, gán luôn
    }

#if GAZEBO_MAJOR_VERSION > 2
    joint->SetPosition(0, current);
#else
    joint->SetPosition(0, current);
#endif
  }

  // Finalize the controller
  void RosLiftLifter::FiniChild()
  {
    alive_ = false;
    queue_.clear();
    queue_.disable();
    gazebo_ros_->node()->shutdown();
    callback_queue_thread_.join();
  }

  void RosLiftLifter::cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg)
  {
    boost::mutex::scoped_lock scoped_lock(lock);
    lift_height_cmd_ = std::min(lift_height_maximum_, cmd_msg->position.z);
    lift_rotate_cmd_ = tf::getYaw(cmd_msg->orientation);
  }

  void RosLiftLifter::QueueThread()
  {
    static const double timeout = 0.01;

    while (alive_ && gazebo_ros_->node()->ok()) {
      queue_.callAvailable(ros::WallDuration(timeout));
    }
  }

  void RosLiftLifter::UpdateForkEncoder()
  {
#if GAZEBO_MAJOR_VERSION >= 8
    common::Time current_time = parent->GetWorld()->SimTime();
#else
    common::Time current_time = parent->GetWorld()->GetSimTime();
#endif
    double step_time = (current_time - last_encoder_update_).Double();
    last_encoder_update_ = current_time;
#if GAZEBO_MAJOR_VERSION >= 8
    ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->RelativePose();
#else
    ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->GetRelativePose().Ign();
#endif
    lift_height_encoder_ = pose.Pos().Z();
  }
  GZ_REGISTER_MODEL_PLUGIN(RosLiftLifter)
}

