//doxygen
/*! \file */


#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <unistd.h>
#include "visageVision.h"
#include "Common.h"
#include "term.h"
#include <math.h>
#include <sys/timeb.h>
#include "VisageDrawing.h"
#include "LicenseString.h"
#include <string>

using namespace VisageSDK;

VisageTracker *m_Tracker = 0;

IplImage *framePtr = 0;

const int MAX_FACES = 4;
static FaceData trackingData[MAX_FACES];

int *track_stat = 0;

CvCapture* capture;

long trackingTime;

//if user closes window used for displaying results, this will be set to 1 and tracking will stop
bool exitApp;

IplImage *imageBGRA;

/**
 * 
 */
void flushInput(void)
{
    int c;
	
    do 
	{
        c = getchar();
    } while (c != '\n' && c != EOF);
}

/**
* Function for clearing console window. 
*/
void clearScreen(void)
{
	if (!cur_term)
	{
		int result;
		setupterm( NULL, STDOUT_FILENO, &result );
		
		if (result <= 0) return;
	}

  putp( tigetstr( "clear" ) );
}

void displayResults()
{
	//if user closes window used for displaying results, tracking will stop
	if (!cvGetWindowHandle("SimpleTracker"))
	{
		exitApp = true;
		return;
	}

	clearScreen();
	printf("Tracking in progress!\n\n");

	char *tstatus;
	
	for (int i = 0; i < MAX_FACES; i++)
	{
		if (track_stat[i] == TRACK_STAT_OFF)
			tstatus = "OFF";
	}
	
	for (int i = 0; i < MAX_FACES; i++)
	{
		if (track_stat[i] == TRACK_STAT_INIT)
			tstatus = "INITIALIZING";
	}
	
	for (int i = 0; i < MAX_FACES; i++)
	{
		if (track_stat[i] == TRACK_STAT_RECOVERING)
			tstatus = "RECOVERING";
	}
	
	for (int i = 0; i < MAX_FACES; i++)
	{
		if (track_stat[i] == TRACK_STAT_OK)
			tstatus = "OK";
	}

	printf(" Tracking status: %s\n\n", tstatus);

	static float eyeDistance = -1.0f;

	// Example code for accessing specific feature points, in this case nose tip and eyes
	if(track_stat[0] == TRACK_STAT_OK)
	{
		FDP *fp = trackingData[0].featurePoints3DRelative;
		float nosex = fp->getFPPos(9,3)[0];
		float nosey = fp->getFPPos(9,3)[1];
		float nosez = fp->getFPPos(9,3)[2];
		float lex = fp->getFPPos(3,5)[0];
		float ley = fp->getFPPos(3,5)[1];
		float lez = fp->getFPPos(3,5)[2];
		float rex = fp->getFPPos(3,6)[0];
		float rey = fp->getFPPos(3,6)[1];
		float rez = fp->getFPPos(3,6)[2];
		float eyedist = lex -rex;
		int dummy = 0;
	}

	// display the frame rate, position and other info
	float trackingFrameRate = trackingData[0].frameRate;

	if (imageBGRA)
		printf(" %4.1f FPS (track %ld ms) \n Resolution: input %dx%d\n\n",trackingFrameRate, trackingTime, framePtr->width, framePtr->height);
	
	cvShowImage("SimpleTracker",imageBGRA);
	cvWaitKey(10);

	if (imageBGRA)
		cvReleaseImage(&imageBGRA);
}


/**
* Function used for measuring tracker speed. 
* Used for video synchronization (if flag video_file_sync is set to 1)
*
*/
static long getCurrentTimeMs()
{
	struct timeb timebuffer;
	ftime( &timebuffer );
	
	long clockTime = 1000 * (long)timebuffer.time + timebuffer.millitm;

	return clockTime;
}


/**
* The main function for tracking from a video file. 
* It is called when the 'v' is chosen.
*
*/
void trackFromVideo(const char *strAviFile)
{
	double frameTime;
	int frameCount = 0;
	
	//If the flag is set to 1 playback is synchronised by skipping video frames 
	//or introducing delay as needed, so the video file is played at its normal (or desired) speed (set variable targetFps to desired fps)
	bool video_file_sync = true;
	
	long currentTime;
	
	//set to desired fps you wish to achieve from tracker
	float targetFps = 30.0;
	//float targetFps = cvGetCaptureProperty( capture, CV_CAP_PROP_FPS );
	
	cvNamedWindow("SimpleTracker", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("SimpleTracker",100,100);

	capture = cvCaptureFromFile(strAviFile);

	if(!capture)
	{
		printf("Can't read video file.");
		return;
	}
	
	// moment when video started
	long startTime = getCurrentTimeMs();
	
	//frame duration if video fps was equal to targetFps
	frameTime = 1000.0/targetFps;
	
	IplImage *logo = cvLoadImage("../Samples/Linux/data/images/logo.png", CV_LOAD_IMAGE_UNCHANGED);
	
	//tracking is performed while user doesn't close window for displaying results
	while(!exitApp)
	{
		if(!capture) break;
		
		if (!cvGrabFrame( capture )) break;
		
		frameCount++;
		
		if (video_file_sync)
		{
			currentTime = getCurrentTimeMs();
			
			//checking if tracker is too slow
			while((currentTime-startTime) > frameTime*(1+frameCount) )
			{
				if( !cvGrabFrame( capture )) return; //end of stream
						
				frameCount++;
				currentTime = getCurrentTimeMs();
			}	
			
			currentTime = getCurrentTimeMs();
			
			//checking if tracker is too fast (we don't want to risk unnecessary delay so we allow some extra frames, that's why framecount-5)		
			while((currentTime-startTime) < frameTime*(frameCount-5))
			{
				sleep(0.001);
				currentTime = getCurrentTimeMs();
			}
		}
		
		if(!(framePtr = cvRetrieveFrame( capture ))) break;
		
		long startTime = getCurrentTimeMs();
		track_stat = m_Tracker->track(framePtr->width, framePtr->height, (const char*)framePtr->imageData, trackingData, VISAGE_FRAMEGRABBER_FMT_BGR, VISAGE_FRAMEGRABBER_ORIGIN_TL, framePtr->widthStep, frameTime*frameCount, MAX_FACES);
		long endTime = getCurrentTimeMs();
		trackingTime = endTime - startTime;
		
		for (int i = 0; i < MAX_FACES; i++)
		{
			if (track_stat[i] == TRACK_STAT_OK)
			{
				VisageDrawing::drawResults((VsImage*)framePtr, &trackingData[i]);
			}
		}
		
		imageBGRA = cvCreateImage(cvSize(framePtr->width, framePtr->height), IPL_DEPTH_8U, 4);
		cvCvtColor(framePtr, imageBGRA, CV_BGR2BGRA);
		
		if (logo)
		{
			VisageDrawing::drawLogo((VsImage*)imageBGRA, (VsImage*)logo);
		}
		
		displayResults();
	}
	
	cvReleaseCapture(&capture);
	cvReleaseImage(&logo);
}


/**
* The main function for tracking from a video camera connected to the computer.
* It is called when the 'c' is chosen.
*
*/
void trackFromCam()
{
	double cam_width = 640;
	double cam_height = 480;
	double cam_device = 0;
	
	cvNamedWindow("SimpleTracker", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("SimpleTracker",100,100);

	capture = cvCaptureFromCAM( cam_device );
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, cam_width);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, cam_height);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FPS, 30);
	
	if(!capture )
	{
		printf("Can't initialize camera (OpenCV). Please check if your camera is installed properly.\n\n");
		return;
	}
	
	IplImage *logo = cvLoadImage("../Samples/Linux/data/images/logo.png", CV_LOAD_IMAGE_UNCHANGED);
	while(!exitApp)
	{
		if(!capture ) break;
		
		if( !cvGrabFrame( capture )) break;
		
		if(!(framePtr = cvRetrieveFrame( capture ))) break;
			
		cvFlip(framePtr, framePtr, 1);
		long startTime = getCurrentTimeMs();
		track_stat = m_Tracker->track(framePtr->width, framePtr->height, (const char*)framePtr->imageData, trackingData, VISAGE_FRAMEGRABBER_FMT_BGR, VISAGE_FRAMEGRABBER_ORIGIN_TL, 0, -1, MAX_FACES);
		long endTime = getCurrentTimeMs();
		trackingTime = endTime - startTime;
			
		for (int i= 0; i<MAX_FACES; i++)
		{
			if (track_stat[i] == TRACK_STAT_OK)
			{
				VisageDrawing::drawResults((VsImage*)framePtr, &trackingData[i]);
			}
		}
		
		imageBGRA = cvCreateImage(cvSize(framePtr->width, framePtr->height), IPL_DEPTH_8U, 4);;
		cvCvtColor(framePtr, imageBGRA, CV_BGR2BGRA);
		
		if (logo)
		{
			VisageDrawing::drawLogo((VsImage*)imageBGRA, (VsImage*)logo);
		}
		
		displayResults();
	}
	
	cvReleaseCapture(&capture);
	cvReleaseImage(&logo);
}

/*! \mainpage  visage|SDK VisageTrackerDemo example project
*
* \htmlonly
* <table border="0">
* <tr>
* <td width="32"><a href="../../../../../bin/VisageTrackerDemo"><img src="../../../../../doc/images/run_sample.png" border=0 title="Run Sample (this may not work in all browsers, in such case please run VisageTrackerDemo from the visageSDK\bin folder.)"></a></td>
* <td width="32"><a href="../../../../../bin"><img src="../../../../../doc/images/open_bin_folder.png" border=0 title="Open Executable Folder (visageSDK\bin)"></a></td>
* <td width="32"><a href="../../../data"><img src="../../../../../doc/images/open_data_folder.png" border=0 title="Open Data Folder"></a></td>
* <td width="32"><a href="../../../data/video"><img src="../../../../../doc/images/open_folder.png" border=0 title="Open Video Folder"></a></td>
* <td width="32"><a href="../../../build"><img src="../../../../../doc/images/open_project_folder.png" border=0 title="Open Project Folder"></a></td>
* <td width="32"><a href="../../../source/VisageTrackerDemo"><img src="../../../../../doc/images/open_source_folder.png" border=0 title="Open Source Code Folder"></a></td>
* </tr>
* </table>
* \endhtmlonly
*
* \if REDHAT_DOXY 
* \htmlonly
* <h2>Dependencies</h2>
*
* Prerequisites to building the sample project are:
* <ul>
* <li>You should have the Software Development Workstation version of RHEL6 (you can choose the version during the installation of RHEL6)</li>
* <li>OpenGL with dynamic libraries: GL, GLU (yum install mesa-libGL-devel mesa-libGLU-devel)</li>
* <li>GCC: gcc 7.0+ (sudo yum install gcc-c++)</li>
* <li>GTK+ 2.0 with dynamic library: gtk-x11-2.0 (yum install gtk2-devel)</li>
* <ul>
*     <li> provided OpenCV library is built with GTK support</li>
*     <li>libVision library also uses GTK to display error messages</li>
* </ul>
* <li>curses terminal control library (yum install ncurses-devel)</li>
* <li>FFMPEG with dynamic libraries: swscale, avformat.</li> <br>
* FFMPEG libraries which were previously installed using ATRPMS repository are now available on ATRPMS repository mirror server. In order to install them, please do the following steps:
* <ul>
* <li>1. Install RPM repository package (this step can be skipped if you already have ATRPMS repository installed):</li>
* <ul style="list-style-type:none">
*   <li>$> sudo rpm -ivh <a href="http://ftp-stud.fht-esslingen.de/Mirrors/atrpms/dl.atrpms.net/el7-x86_64/atrpms/stable/atrpms-repo-7-7.el7.x86_64.rpm">http://ftp-stud.fht-esslingen.de/Mirrors/atrpms/dl.atrpms.net/el7-x86_64/atrpms/stable/atrpms-repo-7-7.el7.x86_64.rpm</a> </li>
* </ul>
* <li>2. Modify atrpms.repo as follows:</li>
* <ul style="list-style-type:none">
*   <li>$> sudo sed -ri 's/baseurl=(.*)/baseurl=http:\/\/ftp-stud.fht-esslingen.de\/Mirrors\/atrpms\/dl.atrpms.net\/el7-x86_64\/atrpms\/stable/g' /etc/yum.repos.d/atrpms.repo </li>
* </ul>
* <li>3. Run the following command in terminal: </li> 
* <ul style="list-style-type:none">
*   <li>$> sudo yum-config-manager --enable atrpms </li>
* </ul>
* <li>4. Install FFMPEG libraries and create required symlinks: 
* <ul style="list-style-type:none">
*   <li>$> sudo yum install libavformat55.x86_64 </li>
*   <li>$> sudo ln -s /usr/lib64/libavformat.so.55 /usr/lib64/libavformat.so </li>
*   <li>$> sudo yum install libswscale2.x86_64 </li>
*   <li>$> sudo ln -s /usr/lib64/libswscale.so.2 /usr/lib64/libswscale.so </li>
*   <li>$> sudo ln -s /usr/lib64/libjpeg.so.62 /usr/lib64/libjpeg.so </li>
*   <li>$> sudo ln -s /usr/lib64/libavutil.so.52 /usr/lib64/libavutil.so </li>
*   <li>$> sudo ln -s /usr/lib64/libavcodec.so.55 /usr/lib64/libavcodec.so </li>
* </ul>
* </ul>
* </ul>
* \endhtmlonly
* \else
* \htmlonly
* <h2>Dependencies</h2>
*
* Prerequisites to building the sample project are:
* <ul>
* <li>g++ compiler (versions 4.7.x) (sudo apt-get install g++)</li>
* <li>OpenGL with dynamic libraries: GL, GLU (sudo apt-get install freeglut3-dev)</li>
* <li>OpenCV library (sudo apt-get install libopencv-dev)
* <li>Video4Linux library (sudo apt-get install libv4l-dev)</li>
* <li>GTK+ 2.0 with dynamic library: gtk-x11-2.0 (sudo apt-get install libgtk2.0-dev)</li>
* <li>curses terminal control library (sudo apt-get install libncurses5-dev)</li>
* <li>high level programming interface for IEEE1394 digital camera (sudo apt-get install libdc1394-22-dev)</li>
* </ul>
* </p>
* \endhtmlonly
* \endif
* 
* \htmlonly
* <h2>Running prebuilt sample</h2>
* \endhtmlonly
*
* The VisageTrackerDemo example project demonstrates the following capabilities of visage|SDK:
* - real-time head/facial features tracking on multiple faces from video file or camera, with on-the-fly animation, based on the VisageSDK::VisageTracker tracker. Currently, tracking is limited to 4 faces, although this can be modified.
*
* To use the application: 
*
* Run the './runTracker.sh' command from the terminal\n\n
* This is an intentionally simplified version of a tracking application, intended to be used by developers as 
* an introduction. VisageTrackerDemo is a console application, with keyboard based choices made by the
* user. It then starts tracking and displays tracking status and numeric results in the console. Results are also visualized in an additional window as an overlay 
* over the original image.
*
* Classes and methods that are important for detailed understanding how this
* sample project is implemented and how it used visage|SDK are:
* - main(): implements the main user interaction.
* 
* \htmlonly
* <h2>Building the project</h2>
*
* <p>
* The VisageTrackerDemo sample project source file is located in the <a href="../../../source/VisageTrackerDemo">Samples/Linux/source/VisageTrackerDemo</a> subfolder of the visage|SDK for Linux folder.
* It is built using the provided Makefile located in <a href="../../../build/">Samples/Linux/build </a> subfolder.
* </p>
* The application needs to be built with license key in order to work properly. More information on how to obtain license key and include it in the application can be found 
* in <a href="../../../../../doc/licensing.html"><em>Licensing</em></a> section of the documentation.

* \endhtmlonly
*/

/**
* The simple tracker.
*
*/
int main( int argc, const char* argv[] )
{
	std::string videoFilename;
	std::string cfgFile;
	std::string dataPath = "../Samples/data/";
	char source;
	
	exitApp = false;
	
	printf("\nSimple Tracker application\n\n");

	if (argc > 1) 
	{
		cfgFile = argv[1];		
	}
	else
	{
		printf("Error: no configuration argument provided.\nUsage: %s <configuration_filename> [video_file_path]\nwhere video_file_path argument is optional.\n", argv[0]);
		return 1;
	}
	
	if ((cfgFile == "NeuralNet.cfg") || (cfgFile == "Face Detector.cfg"))
	{
		printf("Error: not a valid configuration file.\n");
		return 1;
	}
	
	printf("Track from (v)ideo or (c)am? ");
	scanf("%c", &source); flushInput();
	
	if (argc > 2)
	{
		videoFilename = argv[2];
	}	
	
	if ((source == 'v') && (argc == 2))
	{
		printf("Error: for track from video functionality second argument should be a path to video file.\n");
		return 1;
	}
	
	initializeLicenseManager(licenseKey.c_str());
	
	cfgFile = dataPath + cfgFile;

	m_Tracker = new VisageTracker(cfgFile.c_str()); 

	switch (source) 
	{
		case 'v':
			trackFromVideo(videoFilename.c_str());
			break;
		case 'c':
			trackFromCam();
			break;
		default:
			printf("Wrong choice!\n");
			break;
	}
	
	delete m_Tracker;
	m_Tracker = 0;
	capture = 0;

	return 0;
}
