/***************************************************************************
 *   Copyright (C) 2007 by Mauro,,,					   *
 *   mauro.r@ua.pt							   *
 *                                                                         *
 *   PhUA - Projecto Humanóide da Universidade de Aveiro 07/08		   *
 ***************************************************************************/
#ifndef _VISION_CPP_
#define _VISION_CPP_

/*#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
*/
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <sys/time.h>
#include <signal.h>

#include <highgui.h>
#include <cv.h>
#include <cxcore.h>
#include <cvaux.h>

#include <dc1394_control.h>
#include <raw1394.h>
#include <camera1394_control.h>

#include "mytrig.h"
#include "humanmov.h"
#include "mytime.h"
#include "visiondefs.h"
#include "SerialPort.h"
#include "message.h"
#include "imgwrap.h"


using namespace std;

//---------------------
//- Vari�veis Globais -
//---------------------
int GlobalPos;
int device = 0;
int emptyframe = 0;

// ROI params
int useROI = 0;
CvRect imROI = {0,0,160,120};
int delta = 30;

// ponteiro RS232
Message port;

// Log files
FILE *hsvparam, *vis_log, *pe_log, *t_log, *pot_log;

// Opcoes de execucao
int calib = 0;
int rs232 = 0;
int locate = 0;
int lite = 1;
int exectime = 0;
int poserr = 0;
int poten = 0;
int img_seq = 0;

// Valores default para cor
HSVColour color = {30, 90, 70, 256, 100, 256};

//--------------------------

//------------------------
// Cabe�alhos de Fun��es -
//------------------------
void Interrupt(int);
void SafeExit(int safe=0);
void printHelp();
void printlog(FILE *f, char *pstr);
int calcCenterMass(IplImage* img, CvScalar &cmpos);

void SetSmin(int pos){	color.Slow = pos;}
void SetSmax(int pos){	color.Shigh = pos+1;}
void SetVmin(int pos){	color.Vlow = pos;}
void SetVmax(int pos){	color.Vhigh = pos+1;}
void SetHmax(int pos){	color.Hhigh = pos+1;}
void SetHmin(int pos){	color.Hlow = pos;}

int main(int argc, char *argv[])
{
	/* altera a rotina de atendimento ao ^C */
	struct sigaction sigact;
	sigact.sa_handler = Interrupt;
	sigact.sa_flags = 0;
	if (sigaction(SIGINT, &sigact, 0) == -1)
	{
		perror("Rotina de atendimento não instalada\n");
		exit(EXIT_FAILURE);
	}
//----------------------------------------------------------------------------------------
// Declaracao das variaveis
//----------------------------------------------------------------------------------------
	char c, str[50], slog[30], buffer[25], hour[10], pelog[30], tlog[30], potlog[30], img_name[14];
	signed char q, q1 = tiltoffset, q2 = woffset;
	int nzero, delta, cur_pos[2]={0,0}, indx = 0;
	unsigned long sec, usec, nexec = 0;
	signed char servos[3], pot[30];
	signed char val[3]={0, 0, 0};
	double coord[2];

	//CVision obj1(Gwidth, Gheight, device, Gdepth, Gchannels);
	CvPoint obj_cen, cen, ang, pixdelta;
	CvScalar cmpos;
	int ret;
	camera1394_control camcontrol(0,0,MODE_320x240_YUV422, FRAMERATE_30);

	ang.x=0; ang.y=0;
	cen.x = Gwidth/8; cen.y = Gheight/8;
	obj_cen = cen;

	camcontrol.SetPredefinedParams(setb);
//----------
//- "Menu" -
//----------
	if(argc > 1)
	{
		for(unsigned i = 1; i < argc; i++)
		{
			switch(*argv[i])
			{
				case 'h': printHelp();
					return 0;
				case 'c': calib = 1;
					lite = 0;
					break;
				case 'w': lite = 0;
					break;
				case 's': rs232 = 1;
					break;
				case 'l': locate = 1;
					break;
				case 't': exectime = 1;
					break;
				case 'p': poten = 1;
					rs232 = 1;
					break;
				case 'e': poserr = 1;
					break;
				case 'i': img_seq = 1;
					break;
				default: break;
			}
		}
	}

//----------------------------------------------------------------------------------------
// Criacao dos ficheiros de registo
//----------------------------------------------------------------------------------------
	strcpy(slog, "log");
	getdate(buffer);
	strcat(buffer, ".txt");
	strcat(slog, buffer);
	if((vis_log = fopen(slog,"w")) == NULL){
		cout << "Could not create VIS_LOG file!" << endl;
		return -1;
	}
	printlog(vis_log, "Variables and VIS_LOG created. Program started!\n");

	if(exectime){
		strcpy(tlog, "time");
		strcat(tlog, buffer);
		if((t_log = fopen(tlog,"w")) == NULL){
			printlog(vis_log, "Could not create TIME log file!\n");
	 		exectime = 0;
		}
		fprintf(t_log, "%% inicio\t\t\tfinal\n");
	}
	
	if(poserr){
		strcpy(pelog, "perr");
		strcat(pelog, buffer);
		if((pe_log = fopen(pelog,"w")) == NULL){
			printlog(vis_log, "Could not create POSITION ERROR log file!\n");
			poserr = 0;
		}
		fprintf(pe_log, "%% obj.x\tcen.x\tobj.y\tcen.y\n");
	}
	
	if(poten){
		strcpy(potlog, "pot");
		strcat(potlog, buffer);
		if((pot_log = fopen(potlog,"w")) == NULL){
			printlog(vis_log, "Could not create POTENCIOMETER log file!\n");
			poten = 0;
		}
		fprintf(pot_log, "%% pan\ttilt\twaist\n");
	}

	pop_lookup_table();

//----------------------------------------------------------------------------------------
// Leitura dos parametros armazenados
//----------------------------------------------------------------------------------------
	if((hsvparam = fopen("hsv_param.txt","r")) != NULL){
		fscanf(hsvparam,"HSV Color Detection parameters:\n",str);
		fscanf(hsvparam,"H: %d\t%d\n",&(color.Hlow),&(color.Hhigh));
		fscanf(hsvparam,"S: %d\t%d\n",&(color.Slow),&(color.Shigh));
		fscanf(hsvparam,"V: %d\t%d\n",&(color.Vlow),&(color.Vhigh));
	}else{
		cout << "Could not open file!\n Using default values!\n" << endl;
		printlog(vis_log, "Could not open HSV parameters file!\n");
	}
		fclose(hsvparam);
//----------------------------------------------------------------------------------------
// Criacao das imagens
//----------------------------------------------------------------------------------------
	IplImage* image=cvCreateImage( cvSize(320,240),   IPL_DEPTH_8U, 3 );

// 	IplImage* pyr2 = cvCreateImage(cvSize(Gwidth/2,Gheight/2), Gdepth, 3);//"PyrDown da Original";
 	IplImage* pyr = cvCreateImage(cvSize(160,120), Gdepth, 3);//"PyrDown";
	IplImage* hsv = cvCreateImage(cvSize(160,120), Gdepth, Gchannels);//"Imagem no espaco HSV";

	IplImage* h = cvCreateImage(cvSize(160,120), Gdepth, 1);//"Componente H";
	IplImage* s = cvCreateImage(cvSize(160,120), Gdepth, 1);//"Componente S";
	IplImage* v = cvCreateImage(cvSize(160,120), Gdepth, 1);//"Componente V";

	IplImage* h_filter = cvCreateImage(cvSize(160,120), Gdepth, 1);
	IplImage* s_filter = cvCreateImage(cvSize(160,120), Gdepth, 1);
	IplImage* v_filter = cvCreateImage(cvSize(160,120), Gdepth, 1);

	IplImage* filter = cvCreateImage(cvSize(160,120), Gdepth, 1);
// 	IplImage* erode = cvCreateImage(cvSize(320,240), Gdepth, 1);
// 	IplImage* smooth = cvCreateImage(cvSize(320,240), Gdepth, 1);
// 	IplImage* open = cvCreateImage(cvSize(320,240), Gdepth, 1);
// 	IplImage* close = cvCreateImage(cvSize(320,240), Gdepth, 1);
// 	IplImage* test = cvCreateImage(cvSize(320,240), Gdepth, 1);
	IplImage* final = cvCreateImage(cvSize(160,120), Gdepth, Gchannels);
	IplImage* final_gray= cvCreateImage(cvSize(160,120), Gdepth, 1);

//----------------------------------------------------------------------------------------
// Criação das janelas e trackbars para ajuste fino
//----------------------------------------------------------------------------------------
if(!lite){
	cvNamedWindow("Filtro",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Filtro",580,0);
	cvNamedWindow("Color",CV_WINDOW_AUTOSIZE);
 	cvMoveWindow("Color",910,0);
 	cvNamedWindow("Capture",CV_WINDOW_AUTOSIZE);
 	cvNamedWindow("Pyr",CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("Fill",CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("OpenClose",CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("Test");
}

if(calib){
// 	cvNamedWindow("Color",CV_WINDOW_AUTOSIZE);
//  	cvMoveWindow("Color",910,0);
	cvNamedWindow("H",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("H",250,300);
	cvNamedWindow("S",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("S",580,300);
	cvNamedWindow("V",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("V",910,300);
	
	cvCreateTrackbar("SetHmax","H", &color.Hhigh ,255, SetHmax);
	cvCreateTrackbar("SetHmin","H", &color.Hlow ,255, SetHmin);
	cvCreateTrackbar("SetSmax","S", &color.Shigh ,255, SetSmax);
	cvCreateTrackbar("SetSmin","S", &color.Slow ,255, SetSmin);
	cvCreateTrackbar("SetVmax","V", &color.Vhigh ,255, SetVmax);
	cvCreateTrackbar("SetVmin","V", &color.Vlow ,255, SetVmin);
	
	cvSetTrackbarPos("SetHmax","H",(int)color.Hhigh);
	cvSetTrackbarPos("SetHmin","H",(int)color.Hlow);
	cvSetTrackbarPos("SetSmax","S",(int)color.Shigh);
	cvSetTrackbarPos("SetSmin","S",(int)color.Slow);
	cvSetTrackbarPos("SetVmin","V",(int)color.Vlow);
	cvSetTrackbarPos("SetVmax","V",(int)color.Vhigh);
}

if(rs232){
	//iniciar comm rs232
	if(port.initcom() < 0)
	{
		printlog(vis_log, "Could not open RS232 Comm Port!\n");
		return -1;
	}
	printlog(vis_log, "Comm port opened!\n");
	printlog(vis_log, "Starting comm test!\n");
	if(port.testcom()<0)
	{
		printlog(vis_log, "Comm test failed! Terminating!\n");
		return -1;
	}
	printlog(vis_log, "Comm test passed!\n");
	//seleccionar o controlo do servo da cabeça e inicialização dos seus parametros
	for(unsigned i=0;i<3;i++)
	{
		servos[i]=0;
	}
	servos[0]=1;
	if(port.applyjoint(HEAD, PARAM_SPECIAL, servos) < 0)
		printlog(vis_log, "Could not activate PWM!\n");
	else
		printlog(vis_log, "PWM on!\n");

	for(unsigned i=0;i<3;i++)
	{
		servos[i]=0;
	}
	if(port.applyjoint(HEAD, PARAM_VELOCITY, servos) < 0)
		printlog(vis_log, "Could not initialize velocity parameter value!\n");
	else
		printlog(vis_log, "Velocity parameter initialized!\n");

	for(unsigned i=0;i<3;i++)
	{
		servos[i]=0;
	}
// 	servos[2] = 20;
	if(port.applycontrol(HEAD, PARAM_CONTROLON, servos) < 0)
		printlog(vis_log, "Could not set Controller Type\n");
	if(port.applycontrol(HEAD, PARAM_KI, servos) < 0)
		printlog(vis_log, "Error setting Ki parameter.\n");
	if(port.applycontrol(HEAD, PARAM_KP, servos) < 0)
		printlog(vis_log, "Error setting Kp parameter.\n");
	if(port.applycontrol(HEAD, PARAM_KD, servos) < 0)
		printlog(vis_log, "Error setting Kd parameter.\n");

	val[0]=tiltoffset;
	val[1]=panoffset;
	val[2]=woffset;
	if(port.applyjoint(HEAD, PARAM_POSITION, val) < 0)
		printlog(vis_log, "Error sending position orders!\n");
	else
		printlog(vis_log, "Initial position set!\n");
// 	cout << "To (0,0) Position!\n";
}
	//======================
	//	MAIN LOOP
	//======================
	while(c != 's')
	{
		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\t", sec, usec);}

		ret = camcontrol.GetFrame_dma(image);
		//obj1.Refresh();
// Determinação dos filtros a usar para a cor
// 		cvZero(pyr2);
// 		cvPyrDown(obj1.m_pImg, pyr2, CV_GAUSSIAN_5x5);
		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\t", sec, usec);}

		cvZero(pyr);
		cvPyrDown(image, pyr, CV_GAUSSIAN_5x5);

		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\t", sec, usec);}

		cvZero(hsv);
		cvCvtColor(pyr, hsv, CV_BGR2HSV);
		cvZero(h);cvZero(s);cvZero(v);
		cvSplit(hsv, h, s, v, NULL );
		cvZero(h_filter);
		cvInRangeS(h, cvScalar(color.Hlow), cvScalar(color.Hhigh), h_filter);
		cvZero(s_filter);
		cvInRangeS(s, cvScalar(color.Slow), cvScalar(color.Shigh), s_filter);
		cvZero(v_filter);
		cvInRangeS(v, cvScalar(color.Vlow), cvScalar(color.Vhigh), v_filter);	

		cvZero(filter);
		cvAnd(h_filter, s_filter, filter, v_filter);

		cvZero(final);
		cvCopy(pyr, final, filter);

		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\t", sec, usec);}

		if(cvCountNonZero(filter) > 40){
			emptyframe = 0;
			if(((imROI.width - imROI.x) * (imROI.height - imROI.y) < 19200) && useROI)
				cvSetImageROI(filter, imROI);

			if(calcCenterMass(filter, cmpos) != 0)
				printlog(vis_log, "CM was not calculated\n");

			cvResetImageROI(filter);

			if(exectime){
				getsec(sec, usec);
				fprintf(t_log, "%lu%06lu\t", sec, usec);}

			obj_cen.x = (int)cmpos.val[0];
			obj_cen.y = (int)cmpos.val[1];
			pixdelta.x = (obj_cen.x-cen.x);
			pixdelta.y = (obj_cen.y-cen.y);
			ang.x=(int)(-pixdelta.x / 12);
			ang.y=(int)((-pixdelta.y / 9)/mycos(val[0]));

			if(poserr){
				fprintf(pe_log, "%d\t%d\t%d\t%d\t%d\t%d\t%d\n", obj_cen.x, cen.x, obj_cen.y, cen.y, val[1], val[0], val[2]);}

			if(locate){
				RefCoordBallCM_2(val, coord, pixdelta.x, pixdelta.y);
				if(c == 'l')
					cout << "xB= " << coord[0] << "(m)\n" << "yB= " << coord[1] << "(m)" << endl;

			}
			//if(pixdelta.x < 5 && pixdelta.y < 5)
			//	useROI = 1;
		}else{
			ang.x = 0;
			ang.y = 0;
			//useROI = 0;
			//emptyframe++;
			//if(emptyframe > 10){
			//NO OBJECT RESETS SEARCH PARAMETERS
			obj_cen.x = pyr->width/2;
			obj_cen.y = pyr->height/2;

			imROI.x = 0;
			imROI.y = 0;
			imROI.width = 160;
			imROI.height = 120;
			emptyframe = 0;
			//scan(val);
			//}
		}

		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\t", sec, usec);}
// 		no centro da imagem
		if(!lite){
			cvLine(final, cvPoint(final->width/2-8, final->height/2), cvPoint(final->width/2+8, final->height/2), CV_RGB(0,0,128), 2, 8 ,0);
			cvLine(final, cvPoint(final->width/2, final->height/2-8), cvPoint(final->width/2, final->height/2+8), CV_RGB(0,0,128), 2, 8 ,0);

			//ROI
			if(!calib){
			cvRectangle(final, cvPoint(imROI.x, imROI.y), cvPoint(imROI.width, imROI.height), CV_RGB(255,255,0), 2, 8 ,0);}

			if(obj_cen.x<=(final->width-10) && obj_cen.y<=(final->height-10))
			{
// 			no sentido dos XX
				cvLine(final, cvPoint(obj_cen.x-8,obj_cen.y), cvPoint(obj_cen.x+8,obj_cen.y), CV_RGB(0,128,0), 2, 8 ,0);
// 			no sentido dos YY
				cvLine(final, cvPoint(obj_cen.x,obj_cen.y-8), cvPoint(obj_cen.x,obj_cen.y+8), CV_RGB(0,128,0), 2, 8 ,0);
			}
		}

		if(img_seq){
			sprintf(img_name, "./final%04d.jpg", indx++);
			cvSaveImage(img_name,final);}

		if(poten){
			if(port.readjoint(pot, HEAD, PARAM_POSITION) < 0)
				printlog(vis_log, "Could not read Potentiometer value!\n");
			fprintf(pot_log,"%d\t%d\t%d\n", pot[1], pot[0], pot[2]);
			//val[0]=pot[0];
			//val[1]=pot[1];
		}

//--------------------------------------------------------------------------------------------
//- Tracking
//--------------------------------------------------------------------------------------------
		if(rs232){
			val[1] += (signed char)ang.x;
// 			val[0] += (signed char)ang.y;

// 			q = val[0];
// 			if(q > ang_max_tilt){
// 				q1 = ang_max_tilt;
// 				q2 = min((int)ang_max_waist, 2 * (q1 - q) + woffset);
// 			}else
// 			if(q < ang_min_tilt){
// 				q1 = ang_min_tilt;
// 				q2 = max((int)ang_min_waist, 2 * (q1 - q) + woffset);
// 			}else{
// 				q2 = woffset;
// 				q1 = q - q2 + woffset;
// 			}
			if(val[0] > ang_min_tilt || ((abs(ang_min_tilt - val[0]) < 2) && (abs(val[2] - woffset) < 2) && ang.y >= 0)){
				val[0] += (signed char)ang.y;
			}else
				val[2] -= (signed char)ang.y;
			//cout << ang.y << "\t" << abs(val[2] - woffset) << "\t" << abs(ang_min_tilt - val[0]) << endl;
// 			val[0] = q1;
// 			val[2] = q2;
			//======//
			// BSTS //
			//======//
			if(val[2] > ang_max_waist)
				val[2] = ang_max_waist;
			if(val[2] < ang_min_waist)
				val[2] = ang_min_waist;
			if(val[0] > ang_max_tilt)
				val[0] = ang_max_tilt;
			if(val[0] < ang_min_tilt)
				val[0] = ang_min_tilt;
			if(val[1] > ang_max_pan)
				val[1] = ang_max_pan;
			if(val[1] < ang_min_pan)
				val[1] = ang_min_pan;

			if(port.applyjoint(HEAD, PARAM_POSITION, val) < 0)
				printlog(vis_log, "Error sending target position!!\n");
		}
	
		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\n", sec, usec);}

		// Monitorar numero de execucoes
		nexec++;

		if(!lite || calib){
			cvShowImage("Filtro", filter);
	 		cvShowImage("Color", final);
			c = cvWaitKey(50);}
			
		if(calib){
			cvShowImage("Pyr", pyr);
	 		cvShowImage("H", h_filter);
 			cvShowImage("S", s_filter);
 			cvShowImage("V", v_filter);}
			cvShowImage("Capture", image);
		if(locate)
			if(c == 'p')
				cout << "x= " << coord[0] << "(m)\n" << "y= " << coord[1] << "(m)" << endl;
	}

	cvSaveImage("./capture.jpg", image);
	cvSaveImage("./pyr.jpg", pyr);
	cvSaveImage("./mask.jpg", filter);
	cvSaveImage("./h_filter.jpg", h_filter);
	cvSaveImage("./s_filter.jpg", s_filter);
	cvSaveImage("./v_filter.jpg", v_filter);
	cvSaveImage("./final.jpg", final);

	SafeExit(1);

	return EXIT_SUCCESS;
}
//====================================
//
//		END MAIN 
//
//====================================

//------------------------------------------------------------------------------------------------------
//------------------------------------------ Fun��es Auxiliares ----------------------------------------
//------------------------------------------------------------------------------------------------------

//------------------------------------
// Fun��o que imprime o menu de ajuda
//------------------------------------
void printHelp()
{
	cout << "\n--------------------------------------\n";
	cout << "Help command:\n";
	cout << "Usage: vision [option1] [option2] ...\n";
	cout << "\nOptions:\n";
	cout << "h - show help\n";
	cout << "w - show auxiliar window (mask)\n";
	cout << "c - calibration mode\n";
	cout << "s - with serial comm\n";
	cout << "i - create an image sequence\n";
	cout << "t - execution time log\n";
	cout << "p - potenciometer log\n";
	cout << "e - position error log\n";
	cout << "\n--------------------------------------\n";
}

//---------------------------------------------------
// Fun��o auxiliar de escrita num ficheiro de registo
//---------------------------------------------------
void printlog(FILE *f, char *pstr)
{
	char str[100], hour[10];

	if(f == NULL){
		cout << "Null file pointer!\n";
		return;}
	if(pstr == NULL){
		cout << "Null string!\n";
		return;}
	gethour(hour);
	sprintf(str, "%s -\t%s", hour, pstr);
	fprintf(f, str);
}

//---------------------------
// C�lculo do centro de massa
//---------------------------
int calcCenterMass(IplImage* img, CvScalar &cmpos)
{
	int y, x, startx, starty, endy, endx, White;
	CvScalar posx, posy;
	CvRect tempROI;

	IplImage* mx= cvCreateImage(cvSize(img->width, img->height), img->depth, 1);
	IplImage* my= cvCreateImage(cvSize(img->width, img->height), img->depth, 1);
	cvZero(mx);
	cvZero(my);

	BwImage imgA(img);
	BwImage imgx(mx);
	BwImage imgy(my);

	if(img->roi != NULL){
		tempROI = imROI;
		cvSetImageROI(mx, tempROI);
		cvSetImageROI(my, tempROI);
		starty = tempROI.y;
		startx = tempROI.x;
		endy = tempROI.height;
		endx = tempROI.width;
//  		cout << "TEMP__ROI " << tempROI.x << "---" << tempROI.y << "---" << tempROI.width << "---" << tempROI.height << "\n";
	}else{
		starty = 0;
		startx = 0;
		endy = img->height;
		endx = img->width;
// xx		cout << "NO ROI\n";
	}

// 	cout << "Searching for yellow....";
	for(y = starty; y < endy; y++){
// 		cout << "\nLine " << y << "Column ";
		for(x = startx; x < endx; x++){
// 			cout << x << " ";
			if(imgA[y][x] != 0){
				imgx[y][x] = x;
				imgy[y][x] = y;
			}
		}
	}
	posx = cvAvg(mx, img);
	cmpos.val[0] = posx.val[0];
	posy = cvAvg(my, img);
	cmpos.val[1] = posy.val[0];

// cout << "\nCMPOSPOS " << cmpos.val[0] << "-.-.-.-.-" << cmpos.val[1] << endl << endl;
	White=cvCountNonZero(img);
	int perc=(int)((double)White*100/((imROI.width-imROI.x)*(imROI.height-imROI.y)));
//	cout << "NonZero.White: " << White << " Perc White: " << perc << endl << "SquareArea: " << (imROI.width-imROI.x)*(imROI.height-imROI.y) << endl;;

	if(img->roi != NULL)
		if(perc > 30 || perc < 10){
			if(perc == 0);
			else{
			delta = (int)(delta * (double)sqrt((double)perc/20));
// 				cout << "1st delta - " << delta;
				if(delta > 90)
					delta = 90;
// 			cout << " Used delta: " << delta << endl;
			}
		}

	if(White < 40){
			imROI.x = 0;
			imROI.y = 0;
			imROI.width = 160;
			imROI.height = 120;
	}else{
	imROI.x = (int)(cmpos.val[0] - delta);
		if(imROI.x < 0)
			imROI.x = 0;
	imROI.y = (int)(cmpos.val[1] - delta);
		if(imROI.y < 0)
			imROI.y = 0;
	imROI.width = (int)(cmpos.val[0] + delta);
		if(imROI.width > img->width)
			imROI.width = img->width;
	imROI.height = (int)(cmpos.val[1] + delta);
		if(imROI.height > img->height)
			imROI.height = img->height;
	}
// cout << "NEXT__ROI " << imROI.x << "---" << imROI.y << "---" << imROI.width << "---" << imROI.height << "\n";

	return 0;
}

//--------------------------------------------------
// Funções que garantem uma saída segura do programa
//--------------------------------------------------
void Interrupt(int signum)
{
	SafeExit();
}

void SafeExit(int safe)
{
	cvDestroyAllWindows();
	
	if(exectime){	fclose(t_log);}
	if(poserr){	fclose(pe_log);}
	if(poten){	fclose(pot_log);}

	if(calib){
		if((hsvparam = fopen("hsv_param.txt", "w")) == NULL)
			printlog(vis_log, "Could not open HSV Parameters file to save!\n");
		else{
		fprintf(hsvparam, "HSV Color Detection parameters:\n");
		fprintf(hsvparam, "H: %d\t%d\n", color.Hlow,color.Hhigh);
		fprintf(hsvparam, "S: %d\t%d\n", color.Slow,color.Shigh);
		fprintf(hsvparam, "V: %d\t%d\n", color.Vlow,color.Vhigh);
		printf("Parameters saved!\n");
		fclose(hsvparam);}
	}

	if(rs232)
		port.killcom();

	printlog(vis_log, "Program ended!\n");
	fclose(vis_log);

	cout << "Safe exit!" << endl;

	if(!safe)
		exit(EXIT_SUCCESS);
}
#endif