/*
 * blobdetection.cpp
 *
 *  Created on: 23/03/2017
 *      Author: zubair khan
 */

#include <ros/ros.h>
#include <stdlib.h>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <librealsense/rs.hpp>
#include <librealsense/rscore.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <boost/foreach.hpp>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
#include <blobdetection/Followmesrv.h>
#include <blobdetection/Followmemsg.h>
#include <std_msgs/String.h>
#include <sstream>

namespace enc = sensor_msgs::image_encodings;
using namespace std;


//static const char WINDOW[] = "Image Processed";
//static const char RESULT[] = "Tracking";

sensor_msgs::PointCloud2 my_pcl;

//Use method of ImageTransport to create image publisher
image_transport::Publisher pub;
bool hasNewPcl = false;

int LowerH = 170;
int LowerS = 150;
int LowerV = 60;
int UpperH = 179;
int UpperS = 255;
int UpperV = 255;

int posX;
int posY;

geometry_msgs::Twist speed;
ros::Publisher velPub;
blobdetection::Followmemsg follow_msg;
ros::Publisher followmemsg_pub;

bool isStop = false;
bool isStart = false;
bool person = false;
int countPerson = 0;
int COUNT_PERSON_MAX = 1;
int lastPersonSend = 0;
ros::NodeHandle* n;

sensor_msgs::PointCloud2 depth;
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;

// typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

//service

	bool followmesrv(blobdetection::Followmesrv::Request& req,
	blobdetection::Followmesrv::Response& res)
	{
		if (req.function == 0) {
			res.success = true;
			isStop = true;
			isStart = false;
		}else if(req.function == 1 && lastPersonSend == 1) {
			res.success = true;
			isStop = false;
			isStart = true;
		}else{
			res.success = false;
		}
		return true;
	}

	void depthcallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
	{
		my_pcl = *cloud_msg;
		hasNewPcl = true;
		//cout<<"here"<<endl;
	}

	void publishPersonDetected(bool isPersonDetected)
	{
		cout << "Person detected " << isPersonDetected << endl;
		if (isPersonDetected)
		{
			if (follow_msg.person == 1)
			{
				countPerson++;
			}
			else
			{
				countPerson = 0;
				cout << "Change msg person = 1" << endl;
				follow_msg.person = 1;
			}
		}
		else
		{
			if (follow_msg.person == 0)
			{
				countPerson++;
			}
			else
			{
				countPerson = 0;
				cout << "Change msg person = 0" << endl;
				follow_msg.person = 0;
			}
		}
		if (countPerson >= COUNT_PERSON_MAX && lastPersonSend != follow_msg.person)
		{
			followmemsg_pub.publish(follow_msg);
			lastPersonSend = follow_msg.person;
			countPerson = 0;
		}
	}

	float getDepthAt(int x, int y)
	{
		int arrayPosition = y * my_pcl.row_step + x * my_pcl.point_step;
		int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0
		int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4
		int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8

		float X;
		float Y;
		float Z;

		memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
		memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
		memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));
		return Z;
	}

	void getXYZ(int x, int y)
	{
		int arrayPosition = y * my_pcl.row_step + x * my_pcl.point_step;
		int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0
		int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4
		int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8

		float X;
		float Y;
		float Z;

		memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
		memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
		memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));

		geometry_msgs::Point p;

		// put data into the point p
		p.x = X;
		p.y = Y;
		p.z = Z;

		if (!isnan(p.x) || !isnan(p.y) || !isnan(p.z))

		{

		std::cout << "x :" << p.x << "y :" << p.y << "z :" << p.z << std::endl;


		if (isnan(p.x) || isnan(p.z) || p.x > 0.35 || p.x < -0.35 || p.z > 3.20 || p.z < 1.0)
		{
			//ROS_INFO("No Person Detected.");
			speed.linear.x = 0;
			speed.angular.z = 0;
			person = false;
		}
		else
		//ROS_INFO("Person Detected.");
		{
			//Angular
			if (p.x > 0.21 && p.x <= 0.35)
			{
				speed.angular.z = 1.0;
			}
			if (p.x > 0.7 && p.x <= 0.21)
			{
				speed.angular.z = 0.7;
			}
			if (p.x > -0.7 && p.x <= 0.7)
			{
				speed.angular.z = 0.0;
			}
			if (p.x > -0.21 && p.x <= -0.7)
			{
				speed.angular.z = -0.7;
			}
			if (p.x >= -0.35 && p.x <= -0.21)
			{
				speed.angular.z = -1.5;
			}

			//Velocity
			if (p.z > 2.6 && p.z <= 3.2)
			{
				speed.linear.x = 1;
			}
			if (p.z > 1.5 && p.z <= 2.6)
			{
				speed.linear.x = 0.70;
			}
			if (p.z > 0.5 && p.z <= 1.5)
			{
				speed.linear.x = 0.50;
			}
			if (p.z > 0.5 && p.z <= 1.30)
			{
				speed.linear.x = 0.40;
			}
			if (p.z > 0.5 && p.z <= 1.20)
			{
				speed.linear.x = 0.30;
			}
			if (p.z > 0.5 && p.z <= 1.10)
			{
				speed.linear.x = 0.20;
			}


			person = true;
			//follow_msg.person = 1;
			//followmemsg_pub.publish();
		}

		if (isStart == false && isStop == true)
		{
			publishPersonDetected(person);
		}

		if (isStart == true && isStop == false && lastPersonSend == 1)
		{
			ROS_INFO("MOVING.");
			velPub.publish(speed);
			if(person == false)
			{
				countPerson ++;
			}
			else
			{
				countPerson = 0;
			}
			if(countPerson > COUNT_PERSON_MAX)
			{
				isStart = false;
				isStop = true;
			}
		}

		return;
		}
	}

	void blobDetectionCallback(const sensor_msgs::ImageConstPtr& original_image)
	{
		//Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing
		cv_bridge::CvImagePtr cv_ptr;
		try
		{
			cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
		}

		catch (cv_bridge::Exception& e)
		{
			//if there is an error during conversion, display it
			ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
			return;
		}

		cv::Mat img_mask, img_hsv;
		cv::cvtColor(cv_ptr->image, img_hsv, CV_BGR2HSV);
		cv::inRange(img_hsv, cv::Scalar(LowerH, LowerS, LowerV), cv::Scalar(UpperH, UpperS, UpperV), img_mask);

		cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));
		cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));

		cv::erode(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));
		cv::dilate(img_mask, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)));

		//Calculate the moments of the thresholded image
		cv::Moments oMoments = moments(img_mask);

		double dM01 = oMoments.m01;
		double dM10 = oMoments.m10;
		double dArea = oMoments.m00;

		posX = dM10 / dArea;
		posY = dM01 / dArea;

		cv::circle(img_mask, cv::Point(posX, posY), 10, cv::Scalar(0, 255, 255), 2);

		cv::Mat Points;
		cv::findNonZero(img_mask, Points);
		cv::Rect Min_Rect = boundingRect(Points);

		cv::rectangle(img_mask, Min_Rect.tl(), Min_Rect.br(), cv::Scalar(255, 0, 0), 1);

		//Display the image using OpenCV
		cv::imshow("RESULT", img_mask);
		cv::waitKey(3);

		int npts = 0;
		float depth_average = 0;
		for(int i = -25; i < 25; i++)
		{
			for(int j = -25; j < 25; j++)
			{
				if(posY - j > 0 && posY + j < 480 && posX - i  > 0 && posX + i < 640)
			{
					if(getDepthAt(posX + i, posY + j) < 5 && !isnan(getDepthAt(posX + i, posY + j)))
					{
						depth_average += getDepthAt(posX+i, posY+j);
						npts++;
					}
			}

			}
		}
		cv::waitKey(3);
		//Convert the CvImage to a ROS image message and publish it on the "camera/image_processed" topic.
		pub.publish(cv_ptr->toImageMsg());
	}

	int main(int argc, char** argv)
	{

		ros::init(argc, argv, "image_processor");
		ros::NodeHandle nh;
		image_transport::ImageTransport it(nh);

		cv::namedWindow("blob");
		cv::createTrackbar("LowerH", "blob", &LowerH, 179, NULL);
		cv::createTrackbar("UpperH", "blob", &UpperH, 179, NULL);
		cv::createTrackbar("LowerS", "blob", &LowerS, 255, NULL);
		cv::createTrackbar("UpperS", "blob", &UpperS, 255, NULL);
		cv::createTrackbar("LowerV", "blob", &LowerV, 255, NULL);
		cv::createTrackbar("UpperV", "blob", &UpperV, 255, NULL);

		//cv::namedWindow(WINDOW, CV_WINDOW_AUTOSIZE);
		//cv::namedWindow(RESULT, CV_WINDOW_AUTOSIZE);

		ros::Subscriber dep;

		dep = nh.subscribe("/camera/depth_registered/points", 1, depthcallback);
		image_transport::Subscriber sub = it.subscribe("/camera/rgb/image_rect_color", 1, blobDetectionCallback);

		followmemsg_pub = nh.advertise<blobdetection::Followmemsg>("/agv1/bt/followmemsg", 10);
		follow_msg.person == 1;


		ros::ServiceServer service = nh.advertiseService("followmeservice", followmesrv);
		velPub = nh.advertise<geometry_msgs::Twist>("/agv1/cmd_vel", 1);
		pub = it.advertise("camera/image_processed", 1);

		ros::Rate rate(30.0);

		int count = 1;
		while (nh.ok())
		{
			std_msgs::String msg;
			std::stringstream ss;

			if (hasNewPcl && isStop == true || isStart == true)
			{
				getXYZ(posX, posY);
				hasNewPcl = false;
			}
			ros::spinOnce();
			rate.sleep();
		}
	}