#include "ros_kinematics/ros_steer_drive.h"
#include <std_msgs/UInt16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>

ros_kinematics::RosSteerDrive::RosSteerDrive(ros::NodeHandle & nh, const std::string &name)
 : nh_(nh),
   motor_loader_("models", "models::BaseSteerDrive"), 
   encoder_loader_("models", "models::BaseAbsoluteEncoder"),
   last_odom_update_(ros::Time(0)),
   last_actuator_update_(ros::Time(0)),
   publish_time_(ros::Time(0)),
   publish_lastest_time_(ros::Time(0))
{

  nh_priv_ = ros::NodeHandle(nh_, name);
  ROS_INFO("RosSteerDrive get node name is %s", name.c_str());

  bool init_odom_enc;
  nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
  odom_enc_initialized_ = !init_odom_enc;
  
  double schedule_delay, schedule_lastest_delay;
  nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
  nh_priv_.param("publishOdomTF", publishOdomTF_, false);
  nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
  nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
  nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
  nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
  nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
  nh_priv_.param("steerChildFrame", steer_id_, std::string("steer_link"));

  nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
  nh_priv_.param("max_update_rate", update_rate_, 100.0);
  nh_priv_.param("schedule_delay", schedule_delay, 0.005);
  nh_priv_.param("schedule_lastest_delay", schedule_lastest_delay, 0.5);
  nh_priv_.param("odomEncSteeringAngleOffset",odom_enc_steering_angle_offset_, 0.);
  nh_priv_.param("wheelAcceleration", wheel_acceleration_, 0.);
  nh_priv_.param("steering_fix_wheel_distance_x", steering_fix_wheel_distance_x_, 0.68);
  nh_priv_.param("steering_fix_wheel_distance_y", steering_fix_wheel_distance_y_, 0.);
  

  std::string steer_drive;
  nh_priv_.param("steer_drive", steer_drive, std::string("motor_wheel::SteerMotor"));
  try
  {
    steer_motor_ =  motor_loader_.createInstance(steer_drive);
    steer_motor_->initialize(nh_, name + "/" + motor_loader_.getName(steer_drive));
    ros::NodeHandle nh(nh_priv_, motor_loader_.getName(steer_drive));
    nh.param("method", steer_motor_mode_, std::string(""));
  }
  catch(const pluginlib::PluginlibException &ex)
  {
    ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", steer_drive.c_str(), ex.what());
    exit(1);
  }

  std::string traction_drive;
  nh_priv_.param("traction_drive", traction_drive, std::string("motor_wheel::TractionMotor"));
  nh_priv_.param("wheel_diameter", wheel_diameter_, 0.210); 
  try
  {
    traction_motor_ =  motor_loader_.createInstance(traction_drive);
    traction_motor_->initialize(nh_, name + "/" + motor_loader_.getName(traction_drive));
    ros::NodeHandle nh(nh_priv_, motor_loader_.getName(traction_drive));
    nh.param("method", traction_motor_mode_, std::string(""));
  }
  catch(const pluginlib::PluginlibException &ex)
  {
    ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", traction_drive.c_str(), ex.what());
    exit(1);
  }

  nh_.param(name + "/use_encoder", use_abs_encoder, false);
  if(use_abs_encoder)
  {
    std::string abs_encoder;
    nh_priv_.param("absolute_encoder", abs_encoder, std::string("absolute_encoder::Measurements"));
    try
    {
      absolute_encoder_ =  encoder_loader_.createInstance(abs_encoder);
      absolute_encoder_->initialize(nh, "AbsoluteEncoder");
    }
    catch(const pluginlib::PluginlibException &ex)
    {
      ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", abs_encoder.c_str(), ex.what());
      exit(1);
    }
  }

  transform_broadcaster_  = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
  odometry_publisher_     = nh_.advertise<nav_msgs::Odometry>(odometry_topic_, 1);
  odometry_enc_publisher_ = nh_.advertise<nav_msgs::Odometry>(odometry_enc_topic_, 1);
  cmd_vel_subscriber_     = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosSteerDrive::CmdVelCallback, this);
  cmd_vel_feedback_       = nh_.advertise<geometry_msgs::Twist>("cmd_vel_feedback", 1);
  cmd_vel_rb              = nh_.advertise<geometry_msgs::Twist>("cmd_vel_rb", 1);
  steer_angle_pub_        = nh_.advertise<std_msgs::Float32>("steer_angle", 1);
  
  // Initialize update rate stuff
  if (this->update_rate_ > 0.0)
    this->update_period_ = 1.0 / this->update_rate_;
  else
    this->update_period_ = 0.0;
    
  schedule_delay_ = ros::Duration(schedule_delay);
  schedule_lastest_delay_ = ros::Duration(schedule_lastest_delay);

  // Kinematics
  kinematics_param_ptr = boost::make_shared<ros_kinematics::KinematicSteerDriveParameters>();
  kinematics_param_ptr->initialize(nh_);

  // Thread
  callback_thread_ = new boost::thread(&ros_kinematics::RosSteerDrive::UpdateChild,this);
}

ros_kinematics::RosSteerDrive::~RosSteerDrive()
{
  if (callback_thread_)
  {
    callback_thread_->join();
    delete (callback_thread_);
    callback_thread_ = 0;
  }
}

void ros_kinematics::RosSteerDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(callback_mutex_);
  command_struct_.linear.x = cmd_msg->linear.x;
  command_struct_.angular.z = cmd_msg->angular.z;
  scheduleMotorController(true);
}

/**
 * @brief returns true, if publishing of a cmd_vel topic is publishing
*/ 
bool ros_kinematics::RosSteerDrive::isCmdVelTriggered(void)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}

/**
 * @brief returns true, if publishing of a cmd_vel topic is publishing
*/ 
bool ros_kinematics::RosSteerDrive::isCmdVelLastestTriggered(void)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}

/**
 * @brief schedules MotorController function 
 * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
void ros_kinematics::RosSteerDrive::scheduleMotorController(bool schedule)
{
  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
  if(schedule && publish_time_.isZero())
  {
    publish_time_ = ros::Time::now() + schedule_delay_;
    publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
  }
  if(!schedule && !publish_time_.isZero())
  {
    publish_time_ = ros::Time(0);
    // publish_lastest_time_ = ros::Time(0);
  }
}

void ros_kinematics::RosSteerDrive::MotorController(double target_speed, double target_steering_speed, double dt)
{
  // TODO add the accelerations etc. properly
  double applied_speed = target_speed;
  double applied_steering_speed = target_steering_speed;

  if(traction_motor_->Ready() && steer_motor_->Ready())
  {
    double current_speed = traction_motor_->GetVelocity();
    double current_steering_speed = steer_motor_->GetVelocity();

    if(kinematics_param_ptr->isSetup())
    {
      wheel_acceleration_ = kinematics_param_ptr->getWheelAcceleration();
    }

    if (wheel_acceleration_ > 0)
    {
      // TODO
      // applied_speed = ...;
      // applied_steering_speed = ...;
    }
    if(traction_motor_mode_ == std::string("speed_mode")) 
      traction_motor_->SetVelocity(applied_speed);

    if(steer_motor_mode_ == std::string("position_mode")) 
      steer_motor_->SetPosition(applied_steering_speed);
    else if(steer_motor_mode_ == std::string("speed_mode")) 
      steer_motor_->SetVelocity(applied_steering_speed);
  }
  if(!traction_motor_->Ready())
    ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "traction_motor_ is not Ready.");
  if(!steer_motor_->Ready())
    ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "steer_motor_ is not Ready.");
}

void ros_kinematics::RosSteerDrive::publishSteerJoint(double odom_alpha)
{
  ros::Time current_time = ros::Time::now();
  tf::Quaternion qt;
  tf::Vector3 vt;
  qt.setRPY(0, 0, odom_alpha);
  if(kinematics_param_ptr->isSetup())
  {
    steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
    steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
  }
  vt = tf::Vector3(steering_fix_wheel_distance_x_, steering_fix_wheel_distance_y_, 0);
  tf::Transform base_to_steer_link(qt, vt);
  transform_broadcaster_->sendTransform( tf::StampedTransform(base_to_steer_link, current_time, 
                                                              robot_base_frame_, steer_id_));
}

void ros_kinematics::RosSteerDrive::UpdateOdometryEncoder()
{
  ros::Time current_time = ros::Time::now();
  double step_time = ros::Duration(current_time - last_odom_update_).toSec();
  last_odom_update_ = current_time;

  double odom_alpha;
  if(use_abs_encoder)
    odom_alpha = absolute_encoder_->Position();
  else
    odom_alpha = steer_motor_->Position();

  if(kinematics_param_ptr->isSetup())
  {
    odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
    // ROS_WARN("odom_enc_steering_angle_offset_ %f", odom_enc_steering_angle_offset_);
  }
  odom_alpha += odom_enc_steering_angle_offset_;
  
  std_msgs::Float32 msg;
  msg.data = odom_alpha/M_PI*180;
  steer_angle_pub_.publish(msg);
  
  publishSteerJoint(odom_alpha);
  // ROS_WARN_STREAM("Odom alpha " << odom_alpha << " "<< odom_alpha / M_PI *180);
  // Distance travelled drive wheel
  double drive_dist = traction_motor_->GetVelocity() * (wheel_diameter_ / 2) * step_time; 

  double dd = 0.;
  double da = 0.;

  if (fabs(odom_alpha) < 0.000001) // Avoid dividing with a very small number...
  {
    dd = drive_dist;
    da = 0.;
  }
  else
  {
    if(kinematics_param_ptr->isSetup())
    {
      steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
      steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
    }
    double r_stear = steering_fix_wheel_distance_x_ / sin(odom_alpha);
    double r_fix = r_stear * cos(odom_alpha) - steering_fix_wheel_distance_y_;

    dd = r_fix / r_stear * drive_dist; // Adjust the actual forward movement (located between the fixed front wheels) based on the radius of the drive wheel).
    da = drive_dist / r_stear;
  }
  
  // Update the current estimate
  double dx = dd * cos(pose_encoder_.theta + da / 2.);
  double dy = dd * sin(pose_encoder_.theta + da / 2.);

  // Compute odometric pose
  pose_encoder_.x += dx;
  pose_encoder_.y += dy;
  pose_encoder_.theta += da;

  double w = da / step_time;
  
  double v = dd / step_time;
  geometry_msgs::Twist fb;
  fb.linear.x = v;
  fb.angular.z = w;
  cmd_vel_feedback_.publish(fb);

  tf::Quaternion qt;
  tf::Vector3 vt;
  qt.setRPY(0, 0, pose_encoder_.theta);
  vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0);

  odom_enc_.pose.pose.position.x = vt.x();
  odom_enc_.pose.pose.position.y = vt.y();
  odom_enc_.pose.pose.position.z = vt.z();

  odom_enc_.pose.pose.orientation.x = qt.x();
  odom_enc_.pose.pose.orientation.y = qt.y();
  odom_enc_.pose.pose.orientation.z = qt.z();
  odom_enc_.pose.pose.orientation.w = qt.w();

  odom_enc_.twist.twist.angular.z = w;
  odom_enc_.twist.twist.linear.x = v;
  odom_enc_.twist.twist.linear.y = 0.0;

}

void ros_kinematics::RosSteerDrive::PublishOdometry(double step_time)
{
  // getting data form encoder integration
  odom_ = odom_enc_;
  ros::Time current_time = ros::Time::now();
  std::string odom_frame = odometry_frame_;
  std::string base_footprint_frame = robot_base_frame_;
  tf::Quaternion qt;
  tf::Vector3 vt;
  if(publishOdomTF_)
  {
    qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w);
    vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z);
    tf::Transform base_footprint_to_odom(qt, vt);
    transform_broadcaster_->sendTransform(
        tf::StampedTransform(base_footprint_to_odom, current_time,
                              odom_frame, base_footprint_frame));
  }
  // set covariance - TODO, fix this(!)
  odom_.pose.covariance[0] = 0.00001;
  odom_.pose.covariance[7] = 0.00001;
  odom_.pose.covariance[14] = 1000000000000.0;
  odom_.pose.covariance[21] = 1000000000000.0;
  odom_.pose.covariance[28] = 1000000000000.0;
  odom_.pose.covariance[35] = 0.001;

  // set header
  odom_.header.stamp = current_time;
  odom_.header.frame_id = odom_frame;
  odom_.child_frame_id = base_footprint_frame;

  odometry_publisher_.publish(odom_);

  // publish the encoder based odometry
  {
    if (!odom_enc_initialized_)
    {
      pose_encoder_.x = odom_.pose.pose.position.x;
      pose_encoder_.y = odom_.pose.pose.position.y;
      pose_encoder_.theta = tf::getYaw(odom_.pose.pose.orientation);
      odom_enc_initialized_ = true;

      odom_enc_ = odom_;
    }
    std::string odom_enc_child_frame = odometry_enc_child_frame_;
    if(publishOdomTF_)
    {
      qt = tf::Quaternion(odom_enc_.pose.pose.orientation.x, odom_enc_.pose.pose.orientation.y, odom_enc_.pose.pose.orientation.z, odom_enc_.pose.pose.orientation.w);
      vt = tf::Vector3(odom_enc_.pose.pose.position.x, odom_enc_.pose.pose.position.y, odom_enc_.pose.pose.position.z);
      
      tf::Transform odom_enc_child_to_odom(qt, vt);
      transform_broadcaster_->sendTransform(
          tf::StampedTransform(odom_enc_child_to_odom, current_time,
                                odom_frame, odom_enc_child_frame));
    }
    odom_enc_.pose.covariance = odom_.pose.covariance; // TODO...
    odom_enc_.header.stamp = current_time;
    odom_enc_.header.frame_id = odom_frame; // Note - this is typically /world
    odom_enc_.child_frame_id = odom_enc_child_frame;
    odometry_enc_publisher_.publish(odom_enc_);
  }
}

void ros_kinematics::RosSteerDrive::UpdateChild(void)
{
  ros::Rate rate(update_rate_);
  bool stopped;
  while (ros::ok())
  {
    if(traction_motor_->Ready() && steer_motor_->Ready())
    {
      UpdateOdometryEncoder();
    }
    ros::Time current_time = ros::Time::now();
    double seconds_since_last_update = ros::Duration(current_time - last_actuator_update_).toSec();

    if (seconds_since_last_update > update_period_)
    {
      if(traction_motor_->Ready() && steer_motor_->Ready())
      {
        PublishOdometry(seconds_since_last_update);
      }
      // if (publishWheelTF_)
      //   publishWheelTF();
      // if (publishWheelJointState_)
      //   publishWheelJointState();
      double target_steering_angle_speed_saved_;
      if(isCmdVelTriggered())
      {
        double target_wheel_rotation_speed = command_struct_.linear.x / (wheel_diameter_ / 2.0);
        double target_steering_angle_speed = command_struct_.angular.z;
        double odom_alpha;
        if(use_abs_encoder)
          odom_alpha = absolute_encoder_->Position();
        else
          odom_alpha = steer_motor_->Position();

        if(kinematics_param_ptr->isSetup())
        {
          odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
        }
        odom_alpha += odom_enc_steering_angle_offset_;

        geometry_msgs::Twist fb;

        fb.linear.x = command_struct_.linear.x * fabs(cos(odom_alpha));

        cmd_vel_rb.publish(fb);
        
        MotorController(target_wheel_rotation_speed, target_steering_angle_speed, seconds_since_last_update);
        target_steering_angle_speed_saved_ = target_steering_angle_speed;
        scheduleMotorController(false);
        stopped = false;
      }
      if(isCmdVelLastestTriggered() && !stopped)
      {
        if(steer_motor_mode_ == std::string("position_mode")) 
        {
          MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
          MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
        }
        else
        {
          MotorController(0, 0, seconds_since_last_update);
          MotorController(0, 0, seconds_since_last_update);
        }
        stopped = true;
      }
      last_actuator_update_ = current_time;
    }
    rate.sleep();
    ros::spinOnce();
  }
}



