//===================================================================================
// Name        : saliencyDetectionRudinac.cpp
// Author      : Joris van de Weem, joris.vdweem@gmail.com
// Version     : 1.1
// Copyright   : Copyright (c) 2011 LGPL
// Description : C++ implementation of "Maja Rudinac, Pieter P. Jonker. 
// 				"Saliency Detection and Object Localization in Indoor Environments". 
//				ICPR'2010. pp.404~407											  
//===================================================================================
// v1.1: Ported to Robot Operating System (ROS)

#include <saliency_detection/saliencyDetectionRudinac.h>

void saliencyMapRudinac::imageCB(const sensor_msgs::ImageConstPtr& msg_ptr)
{
	cv_bridge::CvImagePtr cv_ptr;
	sensor_msgs::Image salmap_;
	geometry_msgs::Point salientpoint_;

	Mat image_, saliencymap_;
	Point pt_salient;
	double maxVal;

	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);
	}
	catch (cv_bridge::Exception& e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	}
	cv_ptr->image.copyTo(image_);


	saliencymap_.create(image_.size(),CV_8UC1);
	saliencyMapRudinac::calculateSaliencyMap(&image_, &saliencymap_);

	//-- Return most salient point --//
	cv::minMaxLoc(saliencymap_,NULL,&maxVal,NULL,&pt_salient);
	salientpoint_.x = pt_salient.x;
	salientpoint_.y = pt_salient.y;


	//	CONVERT FROM CV::MAT TO ROSIMAGE FOR PUBLISHING
	saliencymap_.convertTo(saliencymap_, CV_8UC1,255);
	fillImage(salmap_, "mono8",saliencymap_.rows, saliencymap_.cols, saliencymap_.step, const_cast<uint8_t*>(saliencymap_.data));

	saliencymap_pub_.publish(salmap_);
	point_pub_.publish(salientpoint_);

	return;
}


void saliencyMapRudinac::calculateSaliencyMap(const Mat* src, Mat* dst)
{
	Size imageSize(128,128);
	Mat srcDown(imageSize,CV_64F);
	Mat magnitudeI(imageSize,CV_64F);
	Mat magnitudeRG(imageSize,CV_64F);
	Mat magnitudeBY(imageSize,CV_64F);
	Mat magnitude(imageSize,CV_64F);
	
	resize(*src, srcDown, imageSize, 0, 0, INTER_LINEAR);
	
	createChannels(&srcDown);
	createSaliencyMap(I,&magnitudeI);
	createSaliencyMap(RG,&magnitudeRG);
	createSaliencyMap(BY,&magnitudeBY);
	
	magnitude= (magnitudeI + magnitudeRG + magnitudeBY);
	GaussianBlur(magnitude, magnitude, Size(5,5), 0, 0, BORDER_DEFAULT);

	//-- Scale to domain [0,1] --//
	double minVal,maxVal;
	minMaxLoc(magnitude, &minVal, &maxVal);
	magnitude = magnitude / maxVal;

	resize(magnitude, *dst, dst->size(), 0, 0, INTER_LINEAR);
}


void saliencyMapRudinac::createChannels(const Mat* src)
{
	
	b.create(src->size(),CV_32F);
	g.create(src->size(),CV_32F);
	r.create(src->size(),CV_32F);
	I.create(src->size(),CV_32F);
	vector<Mat> planes;	
	split(*src, planes);
	Mat rgmax(src->size(),CV_32F);
	Mat rgbmax(src->size(),CV_32F);
	Mat mask(src->size(),CV_32F);

	for(int j=0; j<r.rows;j++)
       	for(int i=0; i<r.cols; i++)
       	{
			b.at<float>(j,i) = planes[0].at<uchar>(j,i);
			g.at<float>(j,i) = planes[1].at<uchar>(j,i);
			r.at<float>(j,i) = planes[2].at<uchar>(j,i);
       	}
	
	I = r+g+b;
	//threshold(I, I, 255, 255, THRESH_TRUNC); // Saturation as in Matlab?
	I = I/3;
		
	rgmax = max(r,g);
	rgbmax = max(rgmax,b);
	
	//-- Prevent that the lowest value is zero, because you cannot divide by zero.
	for(int j=0; j<r.rows;j++)
       	for(int i=0; i<r.cols; i++)
       	{
			if (rgbmax.at<float>(j,i) == 0) rgbmax.at<float>(j,i) = 1;
       	}


	RG = abs(r-g)/rgbmax;
	BY = abs(b - min(r,g))/rgbmax;

	rgbmax = rgbmax/255;
	//-- If max(r,g,b)<0.1 all components should be zero to stop large fluctuations of the color opponency values at low luminance --//
	threshold(rgbmax,mask,.1,1,THRESH_BINARY);
	RG = RG.mul(mask);
	BY = BY.mul(mask);
	I = I.mul(mask);
}



void saliencyMapRudinac::createSaliencyMap(const Mat src, Mat* dst)
{
	vector<Mat> mv;	
	
	Mat realImage(src.size(),CV_64F);
	Mat imaginaryImage(src.size(),CV_64F); imaginaryImage.setTo(0);
	Mat combinedImage(src.size(),CV_64FC2);
	Mat image_DFT;	
	Mat logAmplitude;
	Mat angle(src.size(),CV_64F);
	Mat Magnitude(src.size(),CV_64F);
	Mat logAmplitude_blur;
	
	for(int j=0; j<src.rows;j++){
       	for(int i=0; i<src.cols; i++){
       		realImage.at<double>(j,i) = src.at<float>(j,i);
       	}
	}

			
	mv.push_back(realImage);
	mv.push_back(imaginaryImage);	
	merge(mv,combinedImage);	
	
	dft( combinedImage, image_DFT);
	split(image_DFT, mv);	

	//-- Get magnitude and phase of frequency spectrum --//
	cartToPolar(mv.at(0), mv.at(1), Magnitude, angle, false);
	log(Magnitude,logAmplitude);	
		
	//-- Blur log amplitude with averaging filter --//
	blur(logAmplitude, logAmplitude_blur, Size(3,3), Point(-1,-1), BORDER_DEFAULT);
	exp(logAmplitude - logAmplitude_blur,Magnitude);
	
	polarToCart(Magnitude, angle,mv.at(0), mv.at(1),false);
	merge(mv,image_DFT);
	dft(image_DFT,combinedImage,CV_DXT_INVERSE);

	split(combinedImage,mv);
	cartToPolar(mv.at(0), mv.at(1), Magnitude, angle, false);
	Magnitude = Magnitude.mul(Magnitude);

	Mat tempFloat(src.size(),CV_32F);
	for(int j=0; j<Magnitude.rows;j++)
       	for(int i=0; i<Magnitude.cols; i++)
       		tempFloat.at<float>(j,i) = Magnitude.at<double>(j,i);
	
	*dst = tempFloat;
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "saliencymap");

	saliencyMapRudinac salmapRudinac;

	ros::spin();

	return 0;
}