// ipelaTrack.cpp : Defines the entry point for the console application.
//

/* This code is provided as is without any warranty.

This code communicates with a Sony Ipela SNC-RZ50P network camera using Windows sockets.
Make sure ws2_32.lib is linked. It gets jpeg video from the camera and stores the images in 
OpenCV IplImage format. Uses OpenCV Haar features to detect a face in the video and tracks the 'first' face 
using CamShift. It changes the pan, tilt and zoom of the camera to follow the face.

*/

#include "stdafx.h"


//int _tmain(int argc, _TCHAR* argv[])
//{
//	return 0;
//}




#include "cv.h"
#include "highgui.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

#ifdef _EiC
#define WIN32
#endif


//#include <winsock2.h>

#include <conio.h>
#include <iostream>
#include <fstream>

using namespace std;

static CvMemStorage* storage = 0;
static CvHaarClassifierCascade* cascade = 0;

CvRect* detect_and_draw( IplImage* img );
CvScalar hsv2rgb( float hue );


const char* cascade_name =
    "haarcascade_frontalface_alt.xml";


int main(int argc, char* argv[])
{
	clock_t start_time = 0;//, stop_time;
	cascade_name = "C:/Program Files (x86)/OpenCV/data/haarcascades/haarcascade_frontalface_alt2.xml";
	cascade = (CvHaarClassifierCascade*)cvLoad( cascade_name, 0, 0, 0 );
	storage = cvCreateMemStorage(0);
	CvRect *faceRect;

	SOCKET ipelaSocket;
	SOCKET ipelaSockMov;
	//The sockaddr_in specifies the address of the socket for TCP/IP sockets. 
	sockaddr_in ipelaAddr;
	
	unsigned short ipelaPort = (u_short)80;
	char *ipelaIP;
	ipelaIP = "130.95.116.92";//"130.95.1.10";//"130.95.80.194";//	

	//char msg[300] = "POST /command/ptzf.cgi HTTP/1.1\r\nHost: 130.95.116.92\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Length: 13\r\n\r\nrelative=0605\r\n\r\n";
	//char msg[80] = "GET /command/ptzf.cgi?AreaZoom=120,120,50,50 \r\n\r\n";
	//char *msg = "GET /command/ptzf.cgi?relative=0907 \r\n\r\n";
	//char msg[80] = "GET /oneshotimage.jpg \r\n";
	
	//char home[150] = "GET /command/presetposition.cgi?Homepos=ptz-recall \r\n\r\n";
	char home[1150] = "POST /command/presetposition.cgi HTTP/1.1\r\nHost: 130.95.116.92\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Length: 18\r\n\r\nHomepos=ptz-recall\r\n\r\n";

	
	char msg[80] = "GET /image \r\n";
	char msgMov[800];
	//sprintf(msg,"GET /command/ptzf.cgi?AreaZoom=%d,%d \r\n\r\n",120,120);
	//char msg[180] = "GET /image HTTP/1.0\r\n User-Agent: 130.95.1.174\r\n Host: 130.95.116.92\r\n Accept: */*\r\n Connection: Keep-Alive\r\n \r\n";
		
	WSADATA wsaData;        
    int wsaret=WSAStartup(0x101,&wsaData);
    if(wsaret!=0) 
	{
		printf("WSA startup failed\n");
        return 0;
    }else
		std::cout<<"WSA startup successful"<<std::endl;

    ipelaAddr.sin_family=AF_INET; //Address family    
	ipelaAddr.sin_addr.s_addr = inet_addr(ipelaIP);
	ipelaAddr.sin_port = htons(ipelaPort);	

	int bytesRcvd, x;	//int i;	
	int keyPressed=0;


	ipelaSockMov = socket(AF_INET,SOCK_STREAM,0);						
	if(connect(ipelaSockMov,(sockaddr *)&ipelaAddr,sizeof(ipelaAddr))!=0)
		cout<<"could not connect socket for homing: Error: "<<WSAGetLastError()<<std::endl;					
	else
		send(ipelaSockMov,home,strlen(home),0);										
	//closesocket(ipelaSockMov);

	char msgBuffer[9000]; //header + the image
	char buf2[9000];
	char garbage[5000];
	char command[50]; // only the command part e.g. AreaZoom=x,y,w,h,<codec>

	bytesRcvd = recv(ipelaSockMov, garbage, sizeof(garbage)-1, 0);
	cout<<bytesRcvd<<" bytes"<<endl;
	garbage[bytesRcvd]='\0';
	cout<<garbage<<endl;


	cvNamedWindow("show",1);
	

	ipelaSocket=socket(AF_INET,SOCK_STREAM,0);
	if(ipelaSocket==INVALID_SOCKET)
	{
		printf("invalid socket\n");
		return 0;
	}else
		//std::cout<<"Socket: "<<ipelaSocket<<" created"<<std::endl;
			
	if(connect(ipelaSocket,(sockaddr *)&ipelaAddr,sizeof(ipelaAddr))!=0)
	{
		std::cout<<"could not bind or connect or whatever: Error: "<<WSAGetLastError()<<std::endl;
		return 0;
	}else
		//std::cout<<"Connection successful: "<<std::endl;
	x = send(ipelaSocket,msg,strlen(msg),0);
		std::cout<<"message sent: returned code: "<<x<<": Last Error "<<WSAGetLastError()<<std::endl;

	//string msgStr;
	
	//char bufSock2[9000];

	//throw away the first packet
	bytesRcvd = recv(ipelaSocket, msgBuffer, 9000-1, 0);
	cout<<bytesRcvd<<" bytes: First packet is discarded."<<endl;
		
	bytesRcvd = recv(ipelaSocket, msgBuffer, sizeof(msgBuffer)-1, 0);

	int track_object = 0;
	

	int count = 0;
	int skip = 0;
	ifstream::pos_type size;	
	while(keyPressed != 27) //exit loop on face detection OR Esc key
	{			
		ofstream myFile ("c:/data.jpg", ios::out | ios::binary);														
		
		//string msgStr = msgBuffer; //i = 0;//msgStr.find("--myboundary", 0); //cout<<"boudary "<<i<<endl;
		memcpy(buf2,&msgBuffer[183],bytesRcvd-183); //183 seems to be fixed, if not we can resort to msgStr.find
		myFile.write(buf2, bytesRcvd-183);						
		while(1)
		{				
			bytesRcvd = recv(ipelaSocket, msgBuffer, sizeof(msgBuffer)-1, 0);								
			//string msgStr = msgBuffer; //i = msgStr.find("--myboundary", 0); 	//if(i==string::npos)					
			if(strncmp(msgBuffer,"--myboundary",12))// assumes that --myboundary will always be at the begining of a message
			{				
				//i++; //memcpy(buf2,&msgBuffer[1],bytesRcvd-1); //myFile.write(buf2,bytesRcvd-1);
				myFile.write(msgBuffer, bytesRcvd);					
			}
			else
			{												
				//memcpy(buf2,&msgBuffer[0],i-1); //myFile.write(buf2,i-1);
				break;
			}
		}			
		myFile.close();
		IplImage *img;	// if this and cvReleaseImage are outside the loop, it results in a memory leak
		img = cvLoadImage("c:/data.jpg",1);		
		faceRect = detect_and_draw(img);
		cvSaveImage("g:/Temp/frame0.jpg",img);
		keyPressed = cvWaitKey(10);		
		cvReleaseImage(&img);
		if(faceRect)
		{
			track_object = -1; // set the tracking initialization
			break; //exit the loop on face detection
		}
		if(keyPressed == 27) break; //exit the loop
	}

/**************************************************************************************************************
/**************************************************************************************************************
/*********************** TRACK WITH CAMSHIFT ******************************************************************
/**************************************************************************************************************
/**************************************************************************************************************/
	IplImage *image2 = 0, *hsv = 0, *hue = 0, *mask = 0, *backproject = 0, *histimg = 0;
	CvHistogram *hist = 0;

	int backproject_mode = 0;
	int select_object = 0;
	
	int show_hist = 1;
	int tuneSmin = 1;
	CvPoint origin;
	CvRect selection;					
	CvRect track_window;
	CvBox2D track_box;
	CvConnectedComp track_comp;
	int hdims = 16;
	float hranges_arr[] = {0,180};
	float* hranges = hranges_arr;

	int _vmin = 50, _vmax = 255, smin = 98, val=8; //int _vmin = 25, _vmax = 35, smin = 30, val=8;//

	cvNamedWindow("backProjection",1);
	//cvSetMouseCallback( "backProjection", on_mouse, 0 );
    cvCreateTrackbar( "Vmin", "backProjection", &_vmin, 256, 0 );
    cvCreateTrackbar( "Vmax", "backProjection", &_vmax, 256, 0 );
    cvCreateTrackbar( "Smin", "backProjection", &smin, 256, 0 );

	//_vmin is threshold for dark pixels, _vmax for bright pixels and smin for almost gray pixels (no color)

	selection.x = faceRect->x;        
	selection.y = faceRect->y;
	selection.width = faceRect->width;
	selection.height = faceRect->height;
	//selection.width -= selection.x;
	//selection.height -= selection.y;
	int i, bin_w, c;
	int zoomX, zoomY, aa, bb;
	int frameCounter=0;
	char fileName[50];

	ofstream logFile;
	logFile.open ("g:/Temp/log.txt");
	

	
	while(1) // track continuously, Esc key will exit
	{										
		ofstream myFile ("c:/data.jpg", ios::out | ios::binary);
		memcpy(buf2,&msgBuffer[183],bytesRcvd-183); 
		myFile.write(buf2, bytesRcvd-183);						
		while(1)
		{				
			bytesRcvd = recv(ipelaSocket, msgBuffer, sizeof(msgBuffer)-1, 0);											
			if(strncmp(msgBuffer,"--myboundary",12))// assumes that --myboundary will always be at the begining of a message							
				myFile.write(msgBuffer, bytesRcvd);					
			else							
				break;			
		}			
		myFile.close();
		IplImage *frame;	// if this and cvReleaseImage are outside the loop, it results in a memory leak
		frame = cvLoadImage("c:/data.jpg",1);
																				
		
		if( !image2 )
		{
			/* allocate all the buffers */
			image2 = cvCreateImage( cvGetSize(frame), 8, 3 );
			image2->origin = frame->origin;
			hsv = cvCreateImage( cvGetSize(frame), 8, 3 );
			hue = cvCreateImage( cvGetSize(frame), 8, 1 );
			mask = cvCreateImage( cvGetSize(frame), 8, 1 );
			backproject = cvCreateImage( cvGetSize(frame), 8, 1 );
			hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
			histimg = cvCreateImage( cvSize(320,200), 8, 3 );
			cvZero( histimg );
		}

		cvCopy( frame, image2, 0 );						
		cvReleaseImage(&frame);		
		cvCvtColor( image2, hsv, CV_BGR2HSV );

		
													
		cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0),
					cvScalar(180,256,MAX(_vmin,_vmax),0), mask );
		cvSplit( hsv, hue, 0, 0, 0 );
		

		if( track_object < 0 )
		{
			float max_val = 0.f;
			cvSetImageROI( hue, selection );
			cvSetImageROI( mask, selection );							
			cvCalcHist( &hue, hist, 0, mask );
			cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 );
			cvConvertScale( hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0 );
			cvResetImageROI( hue );
			cvResetImageROI( mask );
			track_window = selection;
			track_object = 1;

			cvZero( histimg );
			bin_w = histimg->width / hdims;
			for( i = 0; i < hdims; i++ )
			{
				val = cvRound( cvGetReal1D(hist->bins,i)*histimg->height/255 );
				CvScalar color = hsv2rgb(i*180.f/hdims);
				cvRectangle( histimg, cvPoint(i*bin_w,histimg->height),
							 cvPoint((i+1)*bin_w,histimg->height - val), color, -1, 8, 0 );
			}
		}

		cvCalcBackProject( &hue, backproject, hist );
		
		//sprintf(fileName, "g:/Temp/back%d.bmp", frameCounter);
		//cvSaveImage(fileName, backproject);
		
		cvAnd( backproject, mask, backproject, 0 );	

		cvShowImage( "backProjection", backproject);


		cvCamShift( backproject, track_window,
					cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
					&track_comp, &track_box );
		track_window = track_comp.rect;
        
		if( backproject_mode )
			cvCvtColor( backproject, image2, CV_GRAY2BGR );
		if( image2->origin )
			track_box.angle = -track_box.angle;
		cvEllipseBox( image2, track_box, CV_RGB(255,0,0), 3, CV_AA, 0 );
		
		logFile <<frameCounter<<" "<<track_box.angle<<" "<<track_box.center.x<<" "<<track_box.center.y<<" "<<track_box.size.width<<" "<<track_box.size.height<<"\n";		

		/*if(tuneSmin == 1 & track_box.size.width > selection.width) 
		{
			smin = smin+5;
		}else
			tuneSmin = 0;
			*/

		//cout<<"width "<<track_box.size.width<<" height "<<track_box.size.height<<endl;

		if( select_object && selection.width > 0 && selection.height > 0 )
		{
			cvSetImageROI( image2, selection );
			cvXorS( image2, cvScalarAll(255), image2, 0 );
			cvResetImageROI( image2 );
		}
		cvShowImage( "show", image2 );//
		frameCounter++;
		sprintf(fileName, "g:/Temp/frame%d.jpg", frameCounter);
		cvSaveImage(fileName, image2);
		keyPressed = cvWaitKey(10);  //the delay ensures display
		if (keyPressed==27) break;	// press Esc to stop	

		//cout<<"zoomX "<<cvRound(track_box.size.width*4/3*1.5)<<" zoomY "<<cvRound(track_box.size.height*1.5)<<endl;
		
		
		/***************************************************************************************
		******************************** Moving the camera *************************************
		****************************************************************************************/
		int tol = 5; // a tolerance value in pixels for camera movement, camera moves only if face move more than tol pixels
		//if(skip>0) // camera is already trying to move, skip 25 frames								
			//skip=skip-1;					
		//else
		
		if((double(clock())-double(start_time))/CLOCKS_PER_SEC > 0.5)//track_box.size.width >= 10 & 
		{
			
			if(track_box.center.x > 320+tol) | (track_box.center.x < 320-tol) | (track_box.center.y > 240+tol) | (track_box.center.y < 240-tol))//count==0)
			{
				count++;
				skip = 15;
				start_time = clock();
				if(track_box.size.height < 400 & track_box.size.width < 400) // zoom only if the face is far
				{
					//zoomX = cvRound(track_box.size.width*4/3*1.2);//cvRound(selection.width*4/3*1.2);
					//zoomY = cvRound(track_box.size.height*1.2);					
					zoomX = cvRound(track_box.size.width);
					if(zoomX < 550) zoomX = 400;//550;

					zoomY = zoomX*3/4;
					sprintf(command,"AreaZoom=%d,%d,%d,%d,jpeg",
						cvRound(track_box.center.x - 320), cvRound(track_box.center.y - 240+50),zoomX, zoomY );
				}
				else
				{
					//zoomX = 0;
					//zoomY = 0;
					//aa = 10; //10 for zoom out, 11 for zoom in, 
					//bb = 01; //01 for 10%, 02 for 15% see manuall for more
					sprintf(command,"relative=1009");
					cout<<command<<endl;
				}
				//cout<<"zoom : "<<zoomX<<" "<<zoomY<<endl;
			//sprintf(msgMov, "GET /command/ptzf.cgi?AreaZoom=%d,%d,%d,%d\r\n\r\n", 
				
				sprintf(msgMov, "POST /command/ptzf.cgi HTTP/1.1\r\nHost: 130.95.116.92\r\nConnection: Keep-Alive\r\nCache-Control: no-cache\r\nContent-Length: %d\r\n\r\n%s\r\n\r\n",strlen(command),command);					
					
				//cout<<msgMov<<endl;
		
				//sprintf(msgMov, "GET /command/ptzf.cgi?relative=1001 \r\n\r\n", aa,bb);// for zooming out by 10%

				
				/*ipelaSockMov = socket(AF_INET,SOCK_STREAM,0);						
				if(connect(ipelaSockMov,(sockaddr *)&ipelaAddr,sizeof(ipelaAddr))!=0)
					cout<<"could not connect 2nd socket: Error: "<<WSAGetLastError()<<std::endl;					
				else*/					
					send(ipelaSockMov,msgMov,strlen(msgMov),0);										
				//closesocket(ipelaSockMov);
					
					
					//garbage[bytesRcvd] = '\0';
					//cout<<garbage<<endl;

					
			}						
			else
			{////keeping the zooming separate from pan,tilt
				
				if(0)//track_box.size.height < 300) // zoom only if the face is far
				{
					aa=11;//zoom in 
					bb=01; //by 10%.... camera is too slow to do any better than this
					sprintf(msgMov, "GET /command/ptzf.cgi?relative=1001 \r\n\r\n", aa,bb);

					ipelaSockMov = socket(AF_INET,SOCK_STREAM,0);						
					if(connect(ipelaSockMov,(sockaddr *)&ipelaAddr,sizeof(ipelaAddr))!=0)
						cout<<"could not connect socket for homing: Error: "<<WSAGetLastError()<<std::endl;					
					else
						send(ipelaSockMov,home,strlen(home),0);										
					closesocket(ipelaSockMov);
				}	
				
			}	
			//stop_time = clock();
				//cout<<"time = "<<(double(stop_time)-double(start_time))/CLOCKS_PER_SEC<<" seconds"<<endl;			
		}
		/***************************************************************************************
		*************************** END OF Moving the camera *************************************
		****************************************************************************************/
		if(track_box.size.width < 10)//frameCounter%100==0) // do a face detection again to update the histogram
		{
			/*ipelaSockMov = socket(AF_INET,SOCK_STREAM,0);						
			if(connect(ipelaSockMov,(sockaddr *)&ipelaAddr,sizeof(ipelaAddr))!=0)
				cout<<"could not connect socket for homing: Error: "<<WSAGetLastError()<<std::endl;					
			else*/
				send(ipelaSockMov,home,strlen(home),0);										
				//closesocket(ipelaSockMov);				

			while(keyPressed != 27) //exit loop on face detection OR Esc key
			{			
				ofstream myFile ("c:/data.jpg", ios::out | ios::binary);														
				
				//string msgStr = msgBuffer; //i = 0;//msgStr.find("--myboundary", 0); //cout<<"boudary "<<i<<endl;
				memcpy(buf2,&msgBuffer[183],bytesRcvd-183); //183 seems to be fixed, if not we can resort to msgStr.find
				myFile.write(buf2, bytesRcvd-183);						
				while(1)
				{				
					bytesRcvd = recv(ipelaSocket, msgBuffer, sizeof(msgBuffer)-1, 0);								
					//string msgStr = msgBuffer; //i = msgStr.find("--myboundary", 0); 	//if(i==string::npos)					
					if(strncmp(msgBuffer,"--myboundary",12))// assumes that --myboundary will always be at the begining of a message
					{				
						//i++; //memcpy(buf2,&msgBuffer[1],bytesRcvd-1); //myFile.write(buf2,bytesRcvd-1);
						myFile.write(msgBuffer, bytesRcvd);					
					}
					else
					{												
						//memcpy(buf2,&msgBuffer[0],i-1); //myFile.write(buf2,i-1);
						break;
					}
				}			
				myFile.close();
				IplImage *img;	// if this and cvReleaseImage are outside the loop, it results in a memory leak
				img = cvLoadImage("c:/data.jpg",1);		
				faceRect = detect_and_draw(img);
				frameCounter++;
				sprintf(fileName, "g:/Temp/frame%d.jpg", frameCounter);
				cvSaveImage(fileName, img);
				
				keyPressed = cvWaitKey(10);		
				cvReleaseImage(&img);
				if(faceRect)
				{
					logFile <<faceRect->height<<"\n";
					track_object = -1; // set the tracking initialization
					//smin = 70;
					tuneSmin = 1; //
					selection.x = faceRect->x;        
					selection.y = faceRect->y;
					selection.width = faceRect->width;
					selection.height = faceRect->height;
					break; //exit the loop on face detection
				}else
					logFile <<"0 \n";
				if(keyPressed == 27) break; //exit the loop
			}			
		}
		if(frameCounter%100==0) recv(ipelaSockMov, garbage, sizeof(garbage)-1, 0); //free up the in buffers every 100 frames
	}// END of track continuously, Esc key will exit

		cvReleaseImage(&image2);
		cvReleaseImage(&hsv);
		cvReleaseImage(&hue);
		cvReleaseImage(&mask);
		cvReleaseImage(&backproject);
		cvReleaseImage(&histimg);	

/**************************************************************************************************************
/**************************************************************************************************************
/*********************** END OF TRACK WITH CAMSHIFT ***********************************************************
/**************************************************************************************************************
/**************************************************************************************************************/
					
	closesocket(ipelaSocket);
	closesocket(ipelaSockMov);
	cvDestroyWindow("show");
	cvDestroyWindow("backProjection");
    //it is safer to call it  to terminate use of WS2_32.DLL 
    WSACleanup();
	logFile.close();
    return 0;
}

CvRect* detect_and_draw( IplImage* img )
{
    static CvScalar colors[] = 
    {
        {{0,255,0}},
        {{0,128,255}},
        {{0,255,255}},
        {{0,0,255}},
        {{255,128,0}},
        {{255,255,0}},
        {{255,0,0}},
        {{255,0,255}}
    };
    
    IplImage* gray = cvCreateImage( cvSize(img->width,img->height), 8, 1 );   
	
    int i;
    cvCvtColor( img, gray, CV_BGR2GRAY );
    
    cvEqualizeHist(gray, gray);
    cvClearMemStorage( storage );

	 CvRect *r=0;

    if( cascade )
    {
        double t = (double)cvGetTickCount();
        CvSeq* faces = cvHaarDetectObjects( gray, cascade, storage,
                                            1.2, 8, CV_HAAR_DO_CANNY_PRUNING,//0,
                                            cvSize(70, 70) );
        
        for( i = 0; i < (faces ? faces->total : 0); i++ )
        {
           r = (CvRect*)cvGetSeqElem( faces, i );
            CvPoint center;
            int radius;
            center.x = cvRound(r->x + r->width*0.5);
            center.y = cvRound(r->y + r->height*0.5);
            radius = cvRound((r->width + r->height)*0.25);
            cvCircle( img, center, radius, colors[i%8], 3, 8, 0 );
        }
    }

    cvShowImage( "show", img );
	
    cvReleaseImage( &gray );   
	return r;
}

CvScalar hsv2rgb( float hue )
{
    int rgb[3], p, sector;
    static const int sector_data[][3]=
        {{0,2,1}, {1,2,0}, {1,0,2}, {2,0,1}, {2,1,0}, {0,1,2}};
    hue *= 0.033333333333333333333333333333333f;
    sector = cvFloor(hue);
    p = cvRound(255*(hue - sector));
    p ^= sector & 1 ? 255 : 0;

    rgb[sector_data[sector][0]] = 255;
    rgb[sector_data[sector][1]] = 0;
    rgb[sector_data[sector][2]] = p;

    return cvScalar(rgb[2], rgb[1], rgb[0],0);
}