#include "map_manager_node.h"

#include "MapIO/map_saver.h"
#include "MapIO/map_loader.h"
#include "tools/loadRosConfig.h"
#include "tools/tools.h"


MapManagerNode::MapManagerNode(ros::NodeHandle *nh)
{
    m_LastLaserTime = ros::Time(0);
    m_Nodehandle = nh;

    int mapSize;
    float resolution;
    loadConfigValue("/homer_mapping/size", mapSize);
    loadConfigValue("/homer_mapping/resolution", resolution);
    m_MapManager = new MapManager(m_Nodehandle);
    m_POIManager = new PoiManager(m_Nodehandle);
    m_MaskingManager = new MaskingManager(mapSize, resolution);

    //subscriptions of MapManagerModule
    m_OctomapSubscriber = m_Nodehandle->subscribe("/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
    m_SLAMMapSubscriber = m_Nodehandle->subscribe("/homer_mapping/slam_map", 1, &MapManagerNode::callbackSLAMMap, this);
    m_SaveMapSubscriber = m_Nodehandle->subscribe("/map_manager/save_map", 1, &MapManagerNode::callbackSaveMap, this);
    m_LoadMapSubscriber = m_Nodehandle->subscribe("/map_manager/load_map", 1, &MapManagerNode::callbackLoadMap, this);
    m_MapVisibilitySubscriber = m_Nodehandle->subscribe("/map_manager/toggle_map_visibility", 1, &MapManagerNode::callbackMapVisibility, this);
    m_LaserScanSubscriber = m_Nodehandle->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);

    //subscriptions of PoiManager
    m_AddPOISubscriber = m_Nodehandle->subscribe("/map_manager/add_POI", 20, &MapManagerNode::callbackAddPOI, this);
    m_ModifyPOISubscriber = m_Nodehandle->subscribe("/map_manager/modify_POI", 100, &MapManagerNode::callbackModifyPOI, this);
    m_DeletePOISubscriber = m_Nodehandle->subscribe("/map_manager/delete_POI", 100, &MapManagerNode::callbackDeletePOI, this);

    //subscriptions of RoiManager
    m_AddROISubscriber = m_Nodehandle->subscribe("/map_manager/add_ROI", 20, &MapManagerNode::callbackAddROI, this);
    m_ModifyROISubscriber = m_Nodehandle->subscribe("/map_manager/modify_ROI", 100, &MapManagerNode::callbackModifyROI, this);
    m_DeleteROIByNameSubscriber = m_Nodehandle->subscribe("/map_manager/delete_ROI_by_name", 100, &MapManagerNode::callbackDeleteROIbyName, this);
    m_DeleteROIByIDSubscriber = m_Nodehandle->subscribe("/map_manager/delete_ROI_by_id", 100, &MapManagerNode::callbackDeleteROIbyID, this);

    //subscribtions of MaskingMapModule
    m_ModifyMapSubscriber = m_Nodehandle->subscribe("/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
    m_ResetMapsSubscriber = m_Nodehandle->subscribe("/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);

    //loaded map publisher
    m_LoadedMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);

    //mask slam publisher
    m_MaskSlamMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);

    m_GetPOIsService = nh->advertiseService("/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
    m_GetROIsService = nh->advertiseService("/map_manager/get_rois", &MapManagerNode::getROIsService, this);

	// Services for Map Handling
    m_SaveMapService 			= nh->advertiseService("/map_manager/save_map", &MapManagerNode::saveMapService, this);
    m_ResetMapService 			= nh->advertiseService("/map_manager/reset_map", &MapManagerNode::resetMapService, this);
    m_LoadMapService 			= nh->advertiseService("/map_manager/load_map", &MapManagerNode::loadMapService, this);

    //load map file from config if present
    std::string mapfile;
    loadConfigValue("/map_manager/default_mapfile", mapfile);
    if(mapfile != "")
    {
		std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
        mapfileMsg->data= mapfile;
        callbackLoadMap ( mapfileMsg );
    }
}

MapManagerNode::~MapManagerNode()
{
    if(m_MapManager) delete m_MapManager;
    if(m_POIManager) delete m_POIManager;
    if(m_MaskingManager) delete m_MaskingManager;
}



void MapManagerNode::callbackSLAMMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
}

void MapManagerNode::callbackOctomapMap( const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
	m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER, msg);
} 

void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr &msg)
{
    MapGenerator map_saver(msg->data);
    nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
    map_saver.save(SLAMMap, maskingMap, m_POIManager->getList());
    //Todo save m_highSensitiveMap
}

void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr &msg)
{
    m_MapManager->clearMapLayers();
    ROS_INFO_STREAM( "Loading map from file " << msg->data);
    MapServer map_loader(msg->data);
    ros::Rate poll_rate(10);
    while(m_LoadedMapPublisher.getNumSubscribers() == 0)
        poll_rate.sleep();
    m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
    nav_msgs::OccupancyGrid::ConstPtr maskingMap = boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getMaskingMap());
    m_MaskingManager->replaceMap(map_loader.getMaskingMap());
    if(maskingMap->data.size() != 0)
        m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
    m_POIManager->replacePOIList(map_loader.getPois());
}

void MapManagerNode::callbackAddPOI(const homer_mapnav_msgs::PointOfInterest::ConstPtr &msg)
{
    m_POIManager->addPointOfInterest(msg);
}

void MapManagerNode::callbackModifyPOI(const homer_mapnav_msgs::ModifyPOI::ConstPtr &msg)
{
    m_POIManager->modifyPointOfInterest(msg);
}

void MapManagerNode::callbackDeletePOI(const homer_mapnav_msgs::DeletePointOfInterest::ConstPtr &msg)
{
    m_POIManager->deletePointOfInterest(msg->name);
}


void MapManagerNode::callbackAddROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr &msg)
{
    m_ROIManager->addRegionOfInterest(msg);
}

void MapManagerNode::callbackModifyROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr &msg)
{
    m_ROIManager->modifyRegionOfInterest(msg);
}

void MapManagerNode::callbackDeleteROIbyName(const std_msgs::String::ConstPtr &msg)
{
    m_ROIManager->deleteRegionOfInterest(msg->data);
}

void MapManagerNode::callbackDeleteROIbyID(const std_msgs::Int32::ConstPtr &msg)
{
    m_ROIManager->deleteRegionOfInterest(msg->data);
}


void MapManagerNode::callbackMapVisibility(const homer_mapnav_msgs::MapLayers::ConstPtr &msg)
{
    m_MapManager->toggleMapVisibility(msg->layer, msg->state);
}

void MapManagerNode::callbackModifyMap(const homer_mapnav_msgs::ModifyMap::ConstPtr &msg)
{
    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->modifyMap(msg);
    if(msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER || msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV)
    {
        m_MaskSlamMapPublisher.publish(maskingMap);
        m_highSensitiveMap = maskingMap;
    }
    else
    {
        m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
    }
}

void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr &msg)
{
    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
}

void MapManagerNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr &msg)
{
	int i;
	int k;
	std::vector<geometry_msgs::Point> laserPoints;
	nav_msgs::OccupancyGrid::Ptr laserMap(new nav_msgs::OccupancyGrid);
	Eigen::Vector2i map_point;
	try
	{	
		ros::Time currentScanTime = ros::Time::now();
		if((currentScanTime - m_LastLaserTime) < ros::Duration(0.5)) return;

        if(m_MapManager->getHeight() == -1) return;
        laserMap->info.height = m_MapManager->getHeight();
        laserMap->info.width = m_MapManager->getWidth();
        laserMap->info.resolution = m_MapManager->getResolution();
        laserMap->info.origin = m_MapManager->getOrigin();
		laserMap->data.resize(laserMap->info.width * laserMap->info.height, homer_mapnav_msgs::ModifyMap::NOT_MASKED );
		laserPoints =  map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, m_TransformListener, msg->header.frame_id, "/map");
		for(i = 0; i < laserPoints.size(); i++)
		{
			map_point = map_tools::toMapCoords(laserPoints.at(i), laserMap->info.origin, laserMap->info.resolution);
			k = map_point.y() * laserMap->info.width + map_point.x();
			if(k < 0)
			{
				return;
			}
			else if (k > laserMap->data.size())
			{
				continue;
			}
			else
			{
				laserMap->data.at(k) = homer_mapnav_msgs::ModifyMap::BLOCKED;
			}
		}
        m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SICK_LAYER, laserMap);
	}
	catch (exception &e)
	{
		ROS_ERROR_STREAM("An error occured: \""<<e.what() << "\"; i: "<<i<<", laserPoints.size(): "<<laserPoints.size()<<", k:"<<k<<", lasermap.data.lenght: 	"<<laserMap->data.size()) ;
	}
}


bool MapManagerNode::getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req,
                                    homer_mapnav_msgs::GetPointsOfInterest::Response& res)
{
    res.poi_list.pois = m_POIManager->getList();
    return true;
}

bool MapManagerNode::getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
                                    homer_mapnav_msgs::GetRegionsOfInterest::Response& res)
{
    res.roi_list.rois = m_ROIManager->getList();
    return true;
}


bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
					homer_mapnav_msgs::SaveMap::Response& res)
{
	//ROS_INFO_STREAM("Saving map "<<req->folder);
    MapGenerator map_saver(std::string(req.folder.data));
    nav_msgs::OccupancyGrid::ConstPtr SLAMMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
    map_saver.save(SLAMMap, maskingMap, m_POIManager->getList() );
}

bool MapManagerNode::loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
					homer_mapnav_msgs::LoadMap::Response& res)
{

    //load map file from config if present
    std::string mapfile = std::string(req.filename.data);
    if(mapfile != "")
    {
		ROS_INFO_STREAM("Loading map with filename: "<<mapfile);
		std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
        mapfileMsg->data= mapfile;
        callbackLoadMap ( mapfileMsg );
    }
	else
	{
		ROS_ERROR_STREAM("Map filename is empty. Could not load map");
	}
}

bool MapManagerNode::resetMapService(std_srvs::Empty::Request& req,
					std_srvs::Empty::Response& res)
{
	ROS_INFO_STREAM("Resetting current map");
    nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
    m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "map_manager");
    ros::NodeHandle nh;

    MapManagerNode node(&nh);

    ros::Rate loop_rate(5);
    while (ros::ok())
    {
        try
        {
            ros::spinOnce();
            loop_rate.sleep();
        }
        catch (exception &e)
        {
            std::cout<<"Exception in main loop"<<e.what()<<std::endl;
        }
    }


    return 0;
}