/** ****************************************************************************************
*  This node presents a fast and precise method to estimate the planar motion of a lidar
*  from consecutive range scans. It is very useful for the estimation of the robot odometry from
*  2D laser range measurements.
*  This module is developed for mobile robots with innacurate or inexistent built-in odometry.
*  It allows the estimation of a precise odometry with low computational cost.
*  For more information, please refer to:
*
*  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
*  Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
******************************************************************************************** */

#include "rf2o_laser_odometry/CLaserOdometry2D.h"
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace std;
using namespace Eigen;

class CLaserOdometry2DNode : CLaserOdometry2D
{
public:

  CLaserOdometry2DNode();
  ~CLaserOdometry2DNode();

  void process();
  void publish();

public:

  bool verbose,publish_tf,new_scan_available;

  double freq;

  std::string         laser_scan_topic;
  std::string         odom_topic;
  std::string         base_frame_id;
  std::string         odom_frame_id;
  std::string         init_pose_from_topic;

  ros::NodeHandle             n;
  sensor_msgs::LaserScan      last_scan;
  bool                        GT_pose_initialized;
  tf::TransformListener       tf_listener;          //Do not put inside the callback
  tf::TransformBroadcaster    odom_broadcaster;
  nav_msgs::Odometry          initial_robot_pose;

  //Subscriptions & Publishers
  ros::Subscriber laser_sub, initPose_sub;
  ros::Publisher odom_pub;

  bool scan_available();

  //CallBacks
  void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan);
  void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose);
};

CLaserOdometry2DNode::CLaserOdometry2DNode() :
  CLaserOdometry2D()
{
    ROS_INFO("Initializing RF2O node...");

    //Read Parameters
    //----------------
    ros::NodeHandle pn("~");
    pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
    pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
    pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
    pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
    pn.param<bool>("publish_tf", publish_tf, true);
    pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
    pn.param<double>("freq",freq,10.0);
    pn.param<bool>("verbose", verbose, true);

    //Publishers and Subscribers
    //--------------------------
    odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
    laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);

    //init pose??
    if (init_pose_from_topic != "")
    {
        initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
        GT_pose_initialized  = false;
    }
    else
    {
        GT_pose_initialized = true;
        initial_robot_pose.pose.pose.position.x = 0;
        initial_robot_pose.pose.pose.position.y = 0;
        initial_robot_pose.pose.pose.position.z = 0;
        initial_robot_pose.pose.pose.orientation.w = 0;
        initial_robot_pose.pose.pose.orientation.x = 0;
        initial_robot_pose.pose.pose.orientation.y = 0;
        initial_robot_pose.pose.pose.orientation.z = 0;
    }

    // Set laser pose on the robot (through tF)
    // This allow estimation of the odometry with respect to the robot base reference system.
    tf::StampedTransform transform;
    transform.setIdentity();
    try
    {
        tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
    }
    catch (tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }

    setLaserPose(transform);

    //Init variables
    module_initialized = false;
    first_laser_scan = true;
}

CLaserOdometry2DNode::~CLaserOdometry2DNode()
{
  //
}

bool CLaserOdometry2DNode::scan_available()
{
    return new_scan_available;
}

void CLaserOdometry2DNode::process()
{
  if( is_initialized() && scan_available() )
  {
      //Process odometry estimation
      odometryCalculation(last_scan);
      publish();
      new_scan_available = false; //avoids the possibility to run twice on the same laser scan
  }
  else
  {
      ROS_WARN("Waiting for laser_scans....") ;
  }
}

//-----------------------------------------------------------------------------------
//                                   CALLBACKS
//-----------------------------------------------------------------------------------

void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
{
    if (GT_pose_initialized)
    {
        //Keep in memory the last received laser_scan
        last_scan = *new_scan;
        current_scan_time = last_scan.header.stamp;

        //Initialize module on first scan
        if (first_laser_scan)
        {
            Init(last_scan, initial_robot_pose.pose.pose);
            first_laser_scan = false;
        }
        else
        {
            //copy laser scan to internal variable
            for (unsigned int i = 0; i<width; i++)
                range_wf(i) = new_scan->ranges[i];
            new_scan_available = true;
        }
    }
}

void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
{
    //Initialize module on first GT pose. Else do Nothing!
    if (!GT_pose_initialized)
    {
        initial_robot_pose = *new_initPose;
        GT_pose_initialized = true;
    }
}

void CLaserOdometry2DNode::publish()
{
  //first, we'll publish the odometry over tf
  //---------------------------------------
  if (publish_tf)
  {
      //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
      geometry_msgs::TransformStamped odom_trans;
      odom_trans.header.stamp = ros::Time::now();
      odom_trans.header.frame_id = odom_frame_id;
      odom_trans.child_frame_id = base_frame_id;
      odom_trans.transform.translation.x = robot_pose.x();
      odom_trans.transform.translation.y = robot_pose.y();
      odom_trans.transform.translation.z = 0.0;
      odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
      //send the transform
      odom_broadcaster.sendTransform(odom_trans);
  }

  //next, we'll publish the odometry message over ROS
  //-------------------------------------------------
  //ROS_INFO("[rf2o] Publishing Odom Topic");
  nav_msgs::Odometry odom;
  odom.header.stamp = ros::Time::now();
  odom.header.frame_id = odom_frame_id;
  //set the position
  odom.pose.pose.position.x = robot_pose.x();
  odom.pose.pose.position.y = robot_pose.y();
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
  //set the velocity
  odom.child_frame_id = base_frame_id;
  odom.twist.twist.linear.x = lin_speed;    //linear speed
  odom.twist.twist.linear.y = 0.0;
  odom.twist.twist.angular.z = ang_speed;   //angular speed
  //publish the message
  odom_pub.publish(odom);
}

//void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
//{
//	//Set the initial pose
//	laser_pose = ini_pose;
//	laser_oldpose = ini_pose;

//    //readLaser(scan);
//	createImagePyramid();
//    //readLaser(scan);
//	createImagePyramid();
//}

//-----------------------------------------------------------------------------------
//                                   MAIN
//-----------------------------------------------------------------------------------
int main(int argc, char** argv)
{
    ros::init(argc, argv, "RF2O_LaserOdom");

    CLaserOdometry2DNode myLaserOdomNode;

    //Main Loop
    //----------
    ROS_INFO("initialization complete...Looping");
    ros::Rate loop_rate(myLaserOdomNode.freq);
    while (ros::ok())
    {
        ros::spinOnce();        //Check for new laser scans

        myLaserOdomNode.process();

        loop_rate.sleep();
    }
    return(0);
}