/*
 * main_nil.cpp
 *
 *  Created on: December  22, 2013
 *      Author: Niladri Das
 *      About: This software package will do the following things:
 *      1. Find out the image coordinates of the the red triangle fixed to the robot.
 */

#include <ros/ros.h>
#include <boost/foreach.hpp>
#include <fstream>
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <math.h>
#include "geometry_msgs/Twist.h"
#include <stdlib.h>
#include <stdio.h>
#include <vector>

using  namespace cv;
using namespace ros;
namespace enc = sensor_msgs::image_encodings;

//static int count_rgb = 0; // global variables for counting
//static int count_depth = 0; // global variables for counting
Mat cannyImg;
Mat dst;
unsigned char* ch;
int check = 1;
int count_topmost = 0;

// RGB call back function

void imageCallback(const sensor_msgs::ImageConstPtr& original_image)
{

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("niladriROSOpenCV::main.cpp::cv_bridge exception:number1:: %s", e.what());
         return;
    }

    cv::Mat imageHsv(cv_ptr->image.rows,cv_ptr->image.cols, CV_8UC3) ;
    cv::cvtColor(cv_ptr->image, imageHsv, CV_BGR2HSV_FULL);
    int rotation = 128 - 255; // 255 = red
    cv::add(imageHsv, cv::Scalar(rotation, 0, 0), imageHsv);
    cv::Mat  imgmask(cv_ptr->image.rows,cv_ptr->image.cols, CV_8UC1);
    cv::inRange(imageHsv, cv::Scalar(98, 135, 135), cv::Scalar(122, 255, 255), imgmask);
    cv::Mat se21 = cv::getStructuringElement(MORPH_RECT, Size(12,12), Point(10,10));
    cv::Mat se11 = cv::getStructuringElement(MORPH_RECT, Size(6,6), Point(5,5));
    dilate(imgmask, imgmask, se21,Point(-1,-1), 1);
    erode (imgmask, imgmask, se21,Point(-1,-1), 1);
    erode(imgmask, imgmask, se11,Point(-1,-1), 1);
//    std::cout<< count_rgb++ << std::endl;
    Canny(imgmask, cannyImg, 50, 150, 3);
    return  ;

}

//// DEPTH call back function
//void imagedepthCallback(const sensor_msgs::ImageConstPtr& depth_image)
//{
//
// 	cv_bridge::CvImagePtr cvdepth_ptr;
//	try
//	{
//	   cvdepth_ptr = cv_bridge::toCvCopy(depth_image,"");
//	}
//	 catch (cv_bridge::Exception& e1)
//    {
//        ROS_ERROR("niladriROSOpenCV::main.cpp::cv_bridge exception:number2 ::%s", e1.what());
//        return;
//    }
//	count_depth++;
//     	return;
//
//}

// MAIN function

int main(int argc, char **argv)
{
	ros::init(argc, argv, "image_processor");
	ros::NodeHandle nh;
	image_transport::ImageTransport it(nh);
//  image_transport::Subscriber sub2 = it.subscribe("camera/depth_registered/image_rect", 1, imagedepthCallback);
	image_transport::Subscriber sub1 = it.subscribe("camera/rgb/image_color", 1, imageCallback);

	while(ros::ok()){
		ros::spinOnce();
	}
//
//	while(check ==1)
//	{
//	printf("Press Control+C");
//	check = 0;
//	}
//   Mat dst;
// We have to check the canny image wich has the same data type as imgmask (8 bit unsigned integer) range 0-255
// Lets covert the 8 bit unsigned canny image to a binary image
	threshold( cannyImg, dst, 10, 255, 0 ); // binary image dst has been formed

//	unsigned char *input = (unsigned char*)(dst.data);
//	int i,j,r,g,b;
//	for(int i = 0;i < dst.rows ;i++){
//	        for(int j = 0;j < dst.cols ;j++){
//	            b = input[dst.step * j + i*(dst.cols*dst.step) ] ;
//	            g = input[dst.step * j + i*(dst.cols*dst.step) + 1];
//	            r = input[dst.step * j + i*(dst.cols*dst.step) + 2];
//	        }
//	    }

//
// Main algorithm for finding three vertices of the triangle
// Four vectors are taken for this algorithm to work
	std::vector<int> upmost (2);
	std::vector<int> downmost (2);
	std::vector<int> leftmost (2);
	std::vector<int> rightmost (2);

// Above vectors need to be properly initialized
	leftmost.at(0) = dst.cols - 1;
	rightmost.at(0) = 0;
//// After processing two of these points will be same, hence 3 distinct points will come
//// Since we are dealing with binary image we can safely assume that dst.step =1
	unsigned char *bin_handle = (unsigned char *)(dst.data);
	int i,j;
	for(i = 0; i < dst.rows;i++)
	{
		for (j = 0; j < dst.cols; j++)
		{
			// First white cell to come is the topmost vertex
			if(bin_handle[j + i*dst.cols] == 255)
			{
				if(count_topmost == 0)
				{
					count_topmost = 1;
					upmost.at(0) = j ;
					upmost.at(1) = i ;
				}

				// For leftmost pixel
				if(j <= leftmost.at(0))
				{
					leftmost.at(0) = j;
					leftmost.at(1) = i;
				}

				// For rightmost pixel
				if(j >= rightmost.at(0))
				{
					rightmost.at(0) = j;
					rightmost.at(1) = i;
				}

				// For the downmost pixel
					downmost.at(0) = j;
					downmost.at(1) = i;
			}

			// Leftmost white cell search

		}

	}
	std::cout << "upmost point  X:" << upmost.at(0) << " Y:" << upmost.at(1) << "\n" << std::endl;
	std::cout << "leftmost point  X:" << leftmost.at(0) << " Y:" << leftmost.at(1) << "\n" << std::endl;
	std::cout << "rightmost point  X:" << rightmost.at(0) << " Y:" << rightmost.at(1) << "\n" << std::endl;
	std::cout << "downmost point  X:" << downmost.at(0) << " Y:" << downmost.at(1) << "\n" << std::endl;
	std::cout << "Total columns:" << dst.cols << "Total rows" << dst.rows << "\n" << std::endl;

//
// Finding out 3 distinct points
// We need to relax the conditions
// Comparison strategy:
	// upmost - leftmost ; upmost - rightmost ; downmost - leftmost ; downmost - rightmost
	std::vector<float> value (4);
	int min_index = 10; // Dummy value given out of range
	value.at(0) = (upmost.at(0)-leftmost.at(0))*(upmost.at(0)-leftmost.at(0)) + (upmost.at(1)-leftmost.at(1))*(upmost.at(1)-leftmost.at(1));
	value.at(1) = (upmost.at(0)-rightmost.at(0))*(upmost.at(0)-rightmost.at(0)) + (upmost.at(1)-rightmost.at(1))*(upmost.at(1)-rightmost.at(1));
	value.at(2) = (downmost.at(0)-leftmost.at(0))*(downmost.at(0)-leftmost.at(0)) + (downmost.at(1)-leftmost.at(1))*(downmost.at(1)-leftmost.at(1));
	value.at(3) = (downmost.at(0)-rightmost.at(0))*(downmost.at(0)-rightmost.at(0)) + (downmost.at(1)-rightmost.at(1))*(downmost.at(1)-rightmost.at(1));

// Finding the index of the minimum value by simple comparison
	for(int i =0 ; i < 3 ; i++)
	{
		for(int j = i+1 ;j <=3-i; j++)
		{
			if(value.at(i) <= value.at(j))
				min_index = i;
			else
				min_index = j;
		}
	}

// Points are to be labelled  1,2,3 from left to right
if(min_index == 0)
	std::cout <<"upmost and leftmost points are the same\n" << std::endl;

else if (min_index == 1)
	std::cout <<"upmost and rightmost points are the same\n" << std::endl;
else if (min_index == 2)
	std::cout <<"downmost and leftmost points are the same\n" << std::endl;
else if (min_index == 3)
	std::cout <<"downmost and rightmost points are the same\n" << std::endl;

//
//
//// showing the 3 vertex found on the binary image
//
//	for(j = 0; j<= 4; j++)
//	{
//		bin_handle[upmost[0] + upmost[1]*dst.cols - 2 + j] = 1;
//		bin_handle[downmost[0] + downmost[1]*dst.cols - 2 + j] = 1;
//		bin_handle[leftmost[0] + leftmost[1]*dst.cols - 2 + j] = 1;
//		bin_handle[rightmost[0] + rightmost[1]*dst.cols - 2 + j] = 1;
//
//	}
//

//
//	imwrite( "Canny_Image.jpg", cannyImg );
//	cv::namedWindow( "Display window", CV_WINDOW_AUTOSIZE );
//	cv::imshow("Display window",cannyImg);
	cv::namedWindow( "Grey Scale", CV_WINDOW_AUTOSIZE );
	cv::imshow("Grey Scale",dst);
//	cv::namedWindow( "Binary", CV_WINDOW_AUTOSIZE );
//	cv::imshow("Binary",dst);
	waitKey(0);

//	ros::spinOnce();
	return 0;

 }