#include <sstream>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include <stdlib.h>

#include "slam_node.h"

//receive:
#include "sensor_msgs/LaserScan.h"
#include "nav_msgs/Odometry.h"
#include "nav_msgs/OccupancyGrid.h"
#include "tf/tf.h"

#include "tools/loadRosConfig.h"

#include "ParticleFilter/SlamFilter.h"
#include "ParticleFilter/HyperSlamFilter.h"
#include "Math/Box2D.h"
#include "OccupancyMap/OccupancyMap.h"

SlamNode::SlamNode(ros::NodeHandle* nh)
  : m_HyperSlamFilter( 0 )
{
    init();

    // subscribe to topics
    m_LaserScanSubscriber = nh->subscribe("/scan", 1, &SlamNode::callbackLaserScan, this);
    m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>("/odom", 1, &SlamNode::callbackOdometry, this);
    m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>("/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
    m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>("/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
    m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
    m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
    m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
    m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);

	m_InitialPoseSubscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &SlamNode::callbackInitialPose, this);

    // advertise topics
    m_PoseStampedPublisher = nh->advertise<geometry_msgs::PoseStamped>("/pose", 1);
    m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
}

void SlamNode::init()
{
    double waitTime;
    loadConfigValue("/particlefilter/wait_time", waitTime);
    m_WaitDuration = ros::Duration(waitTime);
    loadConfigValue("/selflocalization/scatter_var_xy", m_ScatterVarXY);
    loadConfigValue("/selflocalization/scatter_var_theta", m_ScatterVarTheta);

    m_DoMapping = true;

    int particleNum;
    loadConfigValue("/particlefilter/particle_num", particleNum);
    int particleFilterNum;
    loadConfigValue("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
    m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum );

    m_ReferenceOdometryTime = ros::Time(0);
    m_LaserDataTime = ros::Time(0);;

    m_LastLaserMessageId = 0;
    m_LastMapSendTime = ros::Time(0);
    m_LastPositionSendTime = ros::Time(0);
    m_LastMappingTime = ros::Time(0);
}

SlamNode::~SlamNode()
{
  delete m_HyperSlamFilter;
}

void SlamNode::processMeasurements ( ros::Time odoTime, Pose currentOdometryPose )
{
  // laserscan in between current odometry reading and m_ReferenceOdometry
  // -> calculate pose of robot during laser scan
  ros::Duration d1 = m_LaserDataTime - m_ReferenceOdometryTime;
  ros::Duration d2 = odoTime - m_ReferenceOdometryTime;

  float timeFactor;
  if(d1.toSec()==0.0)
      timeFactor = 0.0f;
  else if(d2.toSec()==0.0)
      timeFactor = 1.0f;
  else
      timeFactor = d1.toSec() / d2.toSec();
  ros::Duration duration = ros::Duration(0);

  Pose interpolatedPose = m_ReferenceOdometryPose.interpolate ( currentOdometryPose, timeFactor );
  m_HyperSlamFilter->filter( interpolatedPose, m_LastLaserData, m_LaserDataTime, duration);
}

void SlamNode::resetMaps()
{
  ROS_INFO( "Resetting maps.." );

  delete m_HyperSlamFilter;
  m_HyperSlamFilter = 0;
  init();

  sendMapDataMessage();
  sendPositionDataMessage();
}

void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
{
	m_HyperSlamFilter->resetHigh();

}


void SlamNode::sendPositionDataMessage()
{
  Pose pose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose();

  geometry_msgs::PoseStamped poseMsg;
  //header
  poseMsg.header.stamp = ros::Time::now();
  poseMsg.header.frame_id = "map";

  //position and orientation
  poseMsg.pose.position.x = pose.x();
  poseMsg.pose.position.y = pose.y();
  poseMsg.pose.position.z = 0.0;
  tf::Quaternion quatTF = tf::createQuaternionFromYaw(pose.theta());
  geometry_msgs::Quaternion quatMsg;
  tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion
  poseMsg.pose.orientation = quatMsg;
  m_PoseStampedPublisher.publish(poseMsg);

  //broadcast transform map -> base_link
  tf::Transform transform(quatTF,
                          tf::Vector3(pose.x(), pose.y(), 0.0));
  m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link"));
  m_LastLaserMessageId = 0;
}

void SlamNode::sendMapDataMessage()
{
  std::vector<int8_t> mapData;
  int width, height;
  float resolution;

  OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
  occMap->getOccupancyProbabilityImage (mapData, width, height, resolution);

  if ( width != height )
  {
    ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
  }
  else
  {
    nav_msgs::OccupancyGrid mapMsg;
    std_msgs::Header header;
    header.stamp = ros::Time::now();
    header.frame_id = "map";
    mapMsg.header = header;
    nav_msgs::MapMetaData mapMetaData;
    mapMetaData.width = width;
    mapMetaData.height = height;
    mapMetaData.resolution = resolution;
    mapMetaData.origin.position.x = -height*resolution/2;
    mapMetaData.origin.position.y = -width*resolution/2;
    mapMetaData.origin.orientation.w = 1.0;
    mapMetaData.origin.orientation.x = 0.0;
    mapMetaData.origin.orientation.y = 0.0;
    mapMetaData.origin.orientation.z = 0.0;
    mapMsg.info = mapMetaData;
    mapMsg.data = mapData;

    m_SLAMMapPublisher.publish(mapMsg);
  }
}

void SlamNode::callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg )
{
    Pose userdef_pose(msg->position.x, msg->position.y, tf::getYaw(msg->orientation));
    m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
}

void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    m_LaserDataTime = ros::Time::now();//msg->header.stamp; TODO use msg stamp
    m_LastLaserData = msg;
}

void SlamNode::callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg) {
    ros::Time currentOdometryTime = ros::Time::now();//msg->header.stamp; TODO use msg stamp once cu2wd node publishes time in odometry msg

    float odoX = msg->pose.pose.position.x;
    float odoY = msg->pose.pose.position.y;
    geometry_msgs::Quaternion quat = msg->pose.pose.orientation;
    float odoTheta = tf::getYaw(quat);

    Pose currentOdometryPose ( odoX, odoY, odoTheta );

    // check if we have a laserscan in between two odometry readings (or at the same time)
    bool mappingPossible = ( currentOdometryTime - m_LastMappingTime > m_WaitDuration ) &&
        ( !m_ReferenceOdometryTime.isZero()) &&
        ( m_LaserDataTime >= m_ReferenceOdometryTime ) &&
        ( currentOdometryTime >= m_LaserDataTime );

    if ( mappingPossible )
    {
      ros::Time startTime = ros::Time::now();
      processMeasurements ( currentOdometryTime, currentOdometryPose );
      ros::Time finishTime = ros::Time::now();

      // send map max. every 500 ms
      if ( (finishTime - m_LastMapSendTime).toSec() > 0.5 )
      {
        sendMapDataMessage();
        m_LastMapSendTime = finishTime;
      }
      sendPositionDataMessage();
      m_LastPositionSendTime=finishTime;
      m_LastMappingTime=currentOdometryTime;

      ROS_DEBUG_STREAM( "Pos. data delay: " << (finishTime - startTime).toSec() << "s" );
      ROS_DEBUG_STREAM("Map send Interval: " << ( finishTime - m_LastPositionSendTime ).toSec() << "s" );
    }
    m_ReferenceOdometryPose = currentOdometryPose;
    m_ReferenceOdometryTime = currentOdometryTime;
}

void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr &msg)
{
    m_DoMapping = msg->data;
    m_HyperSlamFilter->setMapping ( m_DoMapping );
    ROS_INFO_STREAM( "Do mapping is set to " << ( m_DoMapping ) );
}

void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr &msg)
{
    resetMaps();
}

void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    float res = msg->info.resolution;
    int height = msg->info.height; // cell size
    int width = msg->info.width; //cell size
    if(height!=width) {
        ROS_ERROR("Height != width in loaded map");
        return;
    }

    //convert map vector from ros format to robbie probability array
    float* map = new float[msg->data.size()];
     //generate exploredRegion
     int minX = INT_MIN;
     int minY = INT_MIN;
     int maxX = INT_MAX;
     int maxY = INT_MAX;
     for(size_t y = 0; y < msg->info.height; y++)
     {
         int yOffset = msg->info.width * y;
         for(size_t x = 0; x < msg->info.width; x++)
         {
             int i = yOffset + x;
             if(msg->data[i] == -1 )
                 map[i] = 0.5;
             else
                 map[i] = msg->data[i]/100.0;

             if(map[i]!=0.5) {
                 if(minX==INT_MIN || minX > (int)x)
                     minX = (int)x;
                 if(minY==INT_MIN || minY > (int)y)
                     minY = (int)y;
                 if(maxX==INT_MAX || maxX < (int)x)
                     maxX = (int)x;
                 if(maxY==INT_MAX || maxY < (int)y)
                     maxY = (int)y;
             }
         }
     }
     Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY );
     OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, exploredRegion);
     m_HyperSlamFilter->setOccupancyMap( occMap );
     m_HyperSlamFilter->setMapping( false ); //is this already done by gui message?
     ROS_INFO_STREAM( "Replacing occupancy map" );
}


void SlamNode::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
    Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y, tf::getYaw(msg->pose.pose.orientation));
    m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
}

void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    m_HyperSlamFilter->applyMasking(msg);
}

/**
 * @brief main function
 */
int main(int argc, char** argv)
{
    ros::init(argc, argv, "homer_mapping");
    ros::NodeHandle nh;

    SlamNode slamNode(&nh);

    ros::Rate loop_rate(12);
    while (ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}