/*
* jellyTrackerRotated.cpp
*
* A Jon Steck Corporation Code
*
* "Please work"
*      -Jon
*  
*--every other frame sample--
* 
* rotated bounding boxes
*/

#include <opencv/cv.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

using namespace cv;
using namespace std;

//hide the local functions in an anon namespace/same as static function
namespace {
    void help(char** av) {
        cout << "The program captures frames from a video file, image sequence (01.jpg, 02.jpg ... 10.jpg) or camera connected to your computer." << endl
             << "Usage:\n" << av[0] << " <video file, image sequence or device number>" << endl
             << "q,Q,esc -- quit" << endl
             << "space   -- save frame" << endl << endl
             << "\tTo capture from a camera pass the device number. To find the device number, try ls /dev/video*" << endl
             << "\texample: " << av[0] << " 0" << endl
             << "\tYou may also pass a video file instead of a device number" << endl
             << "\texample: " << av[0] << " TheKardashiansGoToTheJerseyShore.avi" << endl
             << "\tYou can also pass the path to an image sequence and OpenCV will treat the sequence just like a video." << endl
             << "\texample: " << av[0] << " TheBachorette7.jpg" << endl;
    }

    int process(VideoCapture& capture) {
        time_t start = time(0);
	int n = 0;
        char filename[200];
        string window_name = "video | q or esc to quit";
        cout << "press space to save a picture. q or esc to quit" << endl;
        namedWindow(window_name, WINDOW_NORMAL); //normal window;
        Mat frame;
	int mem[10000][5];
	double speedSizeAngle[10000][3];
	int memStart = 0;
	int memEnd = 0;
	int frameNum = 1;
	int id = 0;
	int jCount = 0;

        for (;;) {
            capture >> frame;
            if (frame.empty())
                break;
            if (frameNum % 2 == 0)
		{
	    	Mat dGrade;
            	resize(frame,dGrade,Size(),.1,.1,INTER_LINEAR);
	    	Rect r(55,15,110,60);
		Rect r2(60,20,100,50);
		Mat vis = dGrade(r2);
	    	Mat small = dGrade(r);
	    	Mat grey;
	    	cvtColor(small,grey,CV_RGB2GRAY);
	    	Mat comp, temp;
	    	temp = Mat::ones(grey.rows,grey.cols,0);
            	temp = temp*255;
	    	comp = temp - grey;
            	Mat topHat, struct1;
            	struct1 =  getStructuringElement(2,Size(12,12),Point(-1,-1));
	    	morphologyEx(comp,topHat,MORPH_TOPHAT,struct1);
            	Mat thresh;
            	threshold(topHat,thresh,10,255,THRESH_BINARY);
            	Mat opened, struct2;
	    	struct2 = getStructuringElement(2,Size(4,4),Point(-1,-1));
	    	morphologyEx(thresh,opened,MORPH_OPEN,struct2);
	    	Rect r3(5,5,100,50);
	    	Mat smaller = opened(r3);

            	vector<vector<Point> > contours;
            	vector<Vec4i> hierarchy;
	    	//vector<Point2f> center(contours.size());
		//vector<float>radius( contours.size() );
            	findContours(smaller,contours,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_SIMPLE,Point(0,0));
		/// Approximate contours to polygons + get bounding rects and circles
  	    	vector<vector<Point> > contours_poly( contours.size() );
            	vector<RotatedRect> minRect( contours.size() );
	    	Mat drawing = Mat::zeros( vis.size(), CV_8UC3 );

	    	for( int i = 0; i < contours.size(); i++ )
     			{ 
			minRect[i] = minAreaRect( Mat(contours[i]) );
			//minEnclosingCircle( (Mat)contours_poly[i], center[i], radius[i] );
			mem[memEnd][0] = frameNum;
			mem[memEnd][1] = id;
			id ++;
			//cout << mem[memEnd][0] << endl;
			approxPolyDP( Mat(contours[i]), contours_poly[i], 3, true );
       			//minEnclosingCircle( (Mat)contours_poly[i], center[i], radius[i] );
			//boundRect[i] = boundingRect( Mat(contours_poly[i]) );
			mem[memEnd][2] = int(minRect[i].center.x);
			mem[memEnd][3] = int(minRect[i].center.y);
			//cout << mem[memEnd][3] << endl;
			mem[memEnd][4] = 0;
			speedSizeAngle[memEnd][0] = 0;
			speedSizeAngle[memEnd][1] = 0;
			speedSizeAngle[memEnd][2] = 0;
			for( int k = memStart; k < memEnd; k++)
				{
				if(mem[memEnd][0]-mem[k][0] > 10)
					{
					memStart++;
					}
				//cout << memStart << endl;
				if(abs(mem[k][2]-mem[memEnd][2]) < 5 && abs(mem[k][3]-mem[memEnd][3]) < 4*(mem[memEnd][0]-mem[k][0]))
					{
					mem[memEnd][1] = mem[k][1];
					mem[memEnd][4] = mem[k][4]+1;
					if(mem[memEnd][3] != 0)
						{
						speedSizeAngle[memEnd][0] = double(mem[memEnd][3]-mem[k][3])/double(mem[memEnd][0]-mem[k][0]);
						//cout << "size: " << minRect[i].size << endl;
                                		//cout << "angle: " << minRect[i].angle << endl;
						if(minRect[i].size.height > minRect[i].size.width)
							{
							speedSizeAngle[memEnd][1] = minRect[i].size.height;
							}
						else
							{
							speedSizeAngle[memEnd][1] = minRect[i].size.width;
							}
						speedSizeAngle[memEnd][2] = minRect[i].angle;
						cout << "id: " << mem[memEnd][1] << endl;
						cout << "speed: " << speedSizeAngle[memEnd][0] << endl;
						cout << "length: " << speedSizeAngle[memEnd][1] << endl;
						cout << "angle = " << speedSizeAngle[memEnd][2] << endl;
						cout << "\n" << endl;
						}
					}
				}
			//cout << mem[memEnd][1] << endl;
			//string String = static_cast<ostringstream*>( &(ostringstream() << mem[memEnd][1]) )->str();
			//putText(small,String,boundRect[i].tl(),FONT_HERSHEY_SIMPLEX,0.1,Scalar(0,0,255),1,8,false);
			if(mem[memEnd][4] == 5)
				{
				jCount++;
				}
			if(mem[memEnd][4]>4)
				{
				for( int j = 0; j < 4; j++)
					{
					Point2f rect_points[4]; minRect[i].points(rect_points);
					line( drawing, rect_points[j], rect_points[(j+1)%4], Scalar(0,0,255), 1, 8);
					}
				//cout << "size: " << minRect[i].size << endl;
				//cout << "angle: " << minRect[i].angle << endl;
				}
			else
				{
				for( int j = 0; j < 4; j++)
                                        {
                                        Point2f rect_points[4]; minRect[i].points(rect_points);
                                        line( drawing, rect_points[j], rect_points[(j+1)%4], Scalar(0,0,100), 1, 8);
                                        }
                                }
			memEnd++;
     			}


  	    /// Draw polygonal contour + bonding rects + circles
  	    //Mat drawing = Mat::zeros( opened.size(), CV_8UC3 );
  	    //for( int i = 0; i< contours.size(); i++ )
     		//{
       		//if(mem[
		//rectangle( drawing, boundRect[i].tl()+Point(5,5), boundRect[i].br()+Point(5,5), Scalar(0,0,100), 1, 8, 0 );
     	    	//}

            	Mat combined;
            	combined = vis + drawing;

	    	string String = static_cast<ostringstream*>( &(ostringstream() << jCount) )->str();
            	putText(combined,String,Point(1,7),FONT_HERSHEY_SIMPLEX,0.3,Scalar(0,0,255),1,8,false);

	    	imshow(window_name, combined);
            	char key = (char)waitKey(3); //delay N millis, usually long enough to display and capture input

            	switch (key) {
            	case 'q':
            	case 'Q':
            	case 27: //escape key
            	    return 0;
            	case ' ': //Save an image
            	    sprintf(filename,"filename%.3d.jpg",n++);
            	    imwrite(filename,frame);
            	    cout << "Saved " << filename << endl;
            	    break;
            	default:
            	    break;
            	}
		}
	frameNum++;
        }
	time_t end = time(0);
	double time = difftime(end,start);
	//cout << speedSizeAngle << endl;
	cout << "You are now " << time << " sec older" << endl;
        return 0;
    }
}

int main(int ac, char** av) {

    if (ac != 2) {
        help(av);
        return 1;
    }
    std::string arg = av[1];
    VideoCapture capture(arg); //try to open string, this will attempt to open it as a video file or image sequence
    if (!capture.isOpened()) //if this fails, try to open as a video camera, through the use of an integer param
        capture.open(atoi(arg.c_str()));
    if (!capture.isOpened()) {
        cerr << "Failed to open the video device, video file or image sequence!\n" << endl;
        help(av);
        return 1;
    }
    return process(capture);
}