#include <stdio.h>
#include <iostream>
#include <cmath>
#include <string>

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/background_segm.hpp"
#include "opencv2\opencv.hpp"
#include "opencv2/core/core.hpp"

using namespace cv;
using namespace std;

int main(int argc, char** argv)
{
	Mat frame;
	Mat gauss_frame;
	Mat fgMaskMOG2;
	Mat binary_frame;
	Mat fg_blur;
	Mat contour_frame;
	bool fine = false;
	bool pausa = false;


	float proporzioni = 80;
	int MIN_WIDTH = 25*proporzioni/100;
	int MIN_HEIGHT = 25*proporzioni/100;
	float MIN_DIST = 70*proporzioni/100;

	int dim_gauss = (proporzioni/100)*5;
	if (dim_gauss%2==0)
		dim_gauss = dim_gauss-1;

	int dim_blur = (proporzioni/100)*40;

	int ingressi = 0;
	int uscite = 0;

	bool prima_frame_con_persone = 1;

	Scalar colore_ing, colore_usc;

	vector <Point> pt_frame_prec;
	vector <Point> pt_frame_now;

	Ptr<BackgroundSubtractorMOG2> pMOG2;
	pMOG2 = new BackgroundSubtractorMOG2(300, 0, true); //history=300frame, soglia:0, shadow_detection = true

	VideoCapture capture(argv[1]);


	while(!fine)
	{
		if (!capture.read(frame))
		{
			cout<<"Fine Video"<<endl;
			fine = true;
		}
		else
		{

			resize(frame, frame, Size(frame.size().width*proporzioni/100, frame.size().height*proporzioni/100) );

			GaussianBlur(frame, gauss_frame, Size(dim_gauss, dim_gauss), 1, 1);

			pMOG2->operator()(gauss_frame, fgMaskMOG2, 0.01);

			blur(fgMaskMOG2, fg_blur, Size(dim_blur, dim_blur));

			threshold(fg_blur, binary_frame, 150, 255, CV_THRESH_BINARY);

			contour_frame = binary_frame.clone();

			vector< vector< Point> > contours;

			findContours(contour_frame, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
			//CV_RETR_EXTERNAL restituisce i contorni esterni
			//CV_CHAIN_APPROX_NONE: tutti i pixel del contorno

			vector< vector< Point> >::iterator itc= contours.begin();

			vector< Point > coordinate;
			Point centro;

			while (itc!=contours.end())
			{
			   Rect temp1 = boundingRect(Mat(*itc));

			   if(temp1.height>MIN_HEIGHT && temp1.width>MIN_WIDTH)
			   {
				   centro.x = temp1.x + temp1.width/2;
				   centro.y = temp1.y + temp1.height/2;

				   coordinate.push_back(centro);

				   rectangle(frame, temp1, Scalar(0,0,255));
				   circle(frame, centro, 2, Scalar( 0, 0, 255 ), 5, 8, 0);
			   }

			   ++itc;
			}



			int confine = frame.size().height/2;

			colore_usc = colore_ing = Scalar(0,0,255);

			if(prima_frame_con_persone)
			{
				pt_frame_prec = coordinate;
				prima_frame_con_persone = 0;
			}
			else
			{
				pt_frame_now = coordinate;

				vector< Point >::iterator itp1, itp2;
				itp1 = pt_frame_now.begin();


				while(itp1!=pt_frame_now.end())
				{
					itp2 = pt_frame_prec.begin();
					Point a = *itp1;
					cout<<"("<<a.x<<", "<<a.y<<")\t";
					while(itp2!=pt_frame_prec.end())
					{
						Point b = *itp2;
						float distanza = sqrt(    pow((float)(a.x-b.x), 2) + pow((float)(a.y - b.y),2)     );
						cout<<distanza<<"\t";
						if(distanza < MIN_DIST )
						{
							line(frame, a, b, Scalar( 0, 255, 0 ), 2, 8, 0);

							if(a.y > confine && b.y <= confine)
							{
								uscite++;
								line(frame, Point(0, confine), Point (frame.size().width, confine-5), Scalar( 255, 255, 0 ), 2, 8, 0);
								colore_usc=Scalar(255,255,0);

							}

							if(a.y <= confine && b.y > confine)
							{
								ingressi++;
								line(frame, Point(0, confine), Point (frame.size().width, confine-5), Scalar( 255, 255, 0 ), 2, 8, 0);
								colore_ing=Scalar(255,255,0);
							}
						}

						++itp2;
					}
					cout<<endl;

					++itp1;
				}
				pt_frame_prec = pt_frame_now;
			}

			stringstream stringaingressi;
			stringstream stringauscite;
			stringaingressi << "Ingressi "<<ingressi;
			stringauscite << "Uscite  "<<uscite;

			if(!pt_frame_now.empty())
					cout<<endl<<stringaingressi.str()<<"\t"<<stringauscite.str()<<endl<<endl;

			putText(frame, stringauscite.str(), Point(2, confine-8), FONT_HERSHEY_SIMPLEX, 0.55, colore_usc, 2, 8, 0);
			putText(frame, stringaingressi.str(), Point(2, confine+18), FONT_HERSHEY_SIMPLEX, 0.55, colore_ing, 2, 8, 0);

			line(frame, Point(0, confine), Point (frame.size().width, confine), Scalar( 0, 255, 0 ), 2, 8, 0);


			imshow ("gaussian", gauss_frame);
			imshow("mog", fgMaskMOG2);
			imshow("fg_blur", fg_blur);
			imshow("binaria", binary_frame);
			imshow("contorni", contour_frame);
			imshow ("prova", frame);


			switch(waitKey(10))
			{
				case 27: //'esc'
					fine = true;
					break;
				case 112: //'p' pausa/riprendi
					pausa = !pausa;
					if(pausa == true)
						cout<<"Code paused, press 'p' again to resume"<<endl;
					while (pausa == true)
					{
						if(waitKey()==112)
						{
							pausa = false;
							cout<<"Code Resumed"<<endl;
						}
					}
					break;
			}
		}
	}
}