/***************************************************************************
 *   Copyright (C) 2007 by Mauro,,,					   *
 *   mauro@mauro-laptop							   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/


#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <iostream>
#include <cstdlib>
#include <ctime>
#include <sys/time.h>

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

#include "cvision.h"
#include "visiondefs.h"
#include "SerialPort.h"
#include "message.h"

#define LITE 0
#define RS232 0

using namespace std;

int getdate(char buffer[]);
int getsec(unsigned long &sec, unsigned long &usec);
int GlobalPos;
int device=0;

HSVColour color = {30, 90, 70, 256, 100, 256};
// HSVColour yellow = {0, 46, 150, 256, 129, 256};

void SetCamnode(int pos){	device = pos;}
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;}

//--------------------------
// Variaveis para o pan tilt
//--------------------------
signed char ang_max_tilt=20;
signed char ang_min_tilt=-40;

signed char ang_max_pan=90;
signed char ang_min_pan=-90;
//--------------------------

int main(int argc, char *argv[])
{
printf("w=%d h=%d nc=%d\n",Gwidth,Gheight,Gchannels);
//****************************************************************************************
// Declaração das variaveis
//****************************************************************************************
	FILE *fp, *log;
	char str[50], slog[23], buffer[17];
	int nzero, delta, calib = 0;
	unsigned long sec, usec;
	signed char val[3]={0, 0, 0}, servos[3];

	Message port;
	CVision obj1(Gwidth, Gheight, device, Gdepth, Gchannels);
	char ball[]="ball\0", c;
	double M00,M10,M01;
	CvPoint obj_cen, cen, ang, cir_cen;
	CvMoments moments;

	cen.x = Gwidth/2;
	cen.y = Gheight/2;
	obj_cen = cen;

//****************************************************************************************
// Determinação do modo de funcionamento
//****************************************************************************************
// 	printf("Working Method?\nc -> Calibration\np -> Processing\n>");
// 	scanf("%c", &c);
// 	switch(c)
// 	{
// 		case 'c':{
// 			printf("Adjust Trackbars and press 's' to save when done!\n");
// 			calib = 1;
// 			sleep(2);
// 			break;
// 		}
// 		case 'p':{
// 			printf("Press 'Ctrl'+'C' to stop execution!\n");
// 			sleep(2);
// 			break;
// 		}
// 		default :{
// 			printf("Invalid Choice! Terminating program!\n");
// 			exit(EXIT_FAILURE);
// 		}
// 	}
c='c';
//****************************************************************************************
// Leitura dos parametros armazenados
//****************************************************************************************
	if((fp = fopen("hsv_param.txt","r")) != NULL)
	{
		fscanf(fp,"HSV Color Detection parameters:\n",str);
		fscanf(fp,"H: %d\t%d\n",&(color.Hlow),&(color.Hhigh));
		fscanf(fp,"S: %d\t%d\n",&(color.Slow),&(color.Shigh));
		fscanf(fp,"V: %d\t%d\n",&(color.Vlow),&(color.Vhigh));
	}
	else
		cout << "Could not open file!" << endl;
	fclose(fp);

//****************************************************************************************
// Criação do ficheiro de registo
//****************************************************************************************
	strcpy(slog, "log");
	getdate(buffer);
	printf("%s\n",buffer);
	strcat(slog, buffer);
	strcat(slog, ".txt");
	printf("%s\n",slog);
	if((log = fopen(slog,"w")) == NULL)
		cout << "Could not create log file!" << endl;

//****************************************************************************************
// Criação das imagens
//****************************************************************************************
	IplImage* pyr2 = cvCreateImage(cvSize(Gwidth/2,Gheight/2), Gdepth, 3);//"PyrDown da Original";
	IplImage* pyr = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 3);//"PyrDown";
	IplImage* hsv = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, Gchannels);//"Imagem no espaço HSV";
	//Componentes da imagem HSV
	IplImage* h = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);//"Componente H";
	IplImage* s = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);//"Componente S";
	IplImage* v = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);//"Componente V";

	IplImage* h_filter = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);
	IplImage* s_filter = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);
	IplImage* v_filter = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);

	IplImage* filter = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);//"Filtro de Cor";
	IplImage* final = cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, Gchannels);
	IplImage* final_gray= cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);

//****************************************************************************************
// Criação das janelas e trackbars para ajuste fino
//****************************************************************************************
if(!LITE){
	cout << "Hello, world!" << endl;
	cvNamedWindow("Pyr",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Pyr",250,0);
	cvNamedWindow("Filtro",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Filtro",580,0);
	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("Device","Pyr", &GlobalPos ,2,SetCamnode);
	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("Device","Pyr",device);
	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){
	//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;
	port.applyjoint(HEAD, PARAM_SPECIAL, servos);
	for(unsigned i=0;i<3;i++)
	{
		servos[i]=10;
	}
	port.applyjoint(HEAD, PARAM_VELOCITY, servos);
}

	while(c != 's')
	{
		getsec(sec, usec);
		fprintf(log, "%lu%06lu\t", sec, usec);
		obj1.Refresh();
	cout << "Captura!" << endl;
//****************************************************************************************
// Determinação dos filtros a usar para a cor
//****************************************************************************************
		cvZero(pyr2);
		cvPyrDown(obj1.m_pImg, pyr2, CV_GAUSSIAN_5x5);
		cvZero(pyr);
		cvPyrDown(pyr2, pyr, CV_GAUSSIAN_5x5);
		if(!LITE)
			cvShowImage("Pyr", pyr);
// 	cvWaitKey();
		cvZero(hsv);
		cvCvtColor(pyr, hsv, CV_BGR2HSV);
	cout << "Conversão!" << endl;
		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);
		if(!LITE)
			cvShowImage("H", h_filter);

		cvZero(s_filter);
		cvInRangeS(s, cvScalar(color.Slow), cvScalar(color.Shigh), s_filter);
		if(!LITE)
			cvShowImage("S", s_filter);

		cvZero(v_filter);
		cvInRangeS(v, cvScalar(color.Vlow), cvScalar(color.Vhigh), v_filter);	
		if(!LITE)
			cvShowImage("V", v_filter);

		cvZero(filter);
		cvAnd(h_filter, s_filter, filter, v_filter);
		if(!LITE)
			cvShowImage("Filtro", filter);

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

		if(cvCountNonZero(filter) > 50)
		{
//****************************************************************************************
// Calculo do centro de massa
//****************************************************************************************
			cvMoments(filter, &moments, 1 );
			M00=cvGetSpatialMoment( &moments, 0, 0 );
			M10=cvGetSpatialMoment( &moments, 1, 0 );
			M01=cvGetSpatialMoment( &moments, 0, 1 );
			if ((((int) (M10/M00)) < pyr->width) && (((int) (M10/M00)) > 0))
				obj_cen.x= ((int) (M10/M00));
			if((((int) (M01/M00)) < pyr->height) && (((int) (M01/M00)) > 0))
				obj_cen.y= ((int) (M01/M00));
//****************************************************************************************
// Verificação do circulo
//****************************************************************************************
			CvMemStorage* storage = cvCreateMemStorage(0);
			cvCvtColor(final, final_gray, CV_BGR2GRAY );
			cvSmooth(final_gray, final_gray, CV_GAUSSIAN, 1, 1 ,0); // smooth it, otherwise a lot of false circles may be detected
			CvSeq* circles = cvHoughCircles(final_gray, storage, CV_HOUGH_GRADIENT, final_gray->height/20,500, 10,1 );
			float* p=NULL;
			char* elem;

			if (( elem = cvGetSeqElem( circles, 1)) > 0)
			{
		 		p = (float*) elem;
        		}
			if(((int)cvRound(p[2]))>10 && ((int) cvRound(p[2]))<100)
			{
				cvCircle(final, cvPoint((int) cvRound(p[0]),(int) cvRound(p[1])),(int) cvRound(p[2]), CV_RGB(128,0,0), 3, 8, 0 );
       			}

			cir_cen.x=cvRound(p[0]);
			cir_cen.y=cvRound(p[1]);

			CvFont font;
			cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 3, 8 );
			static int j=0;

			if(cvRound(p[2])>10 && cvRound(p[2])<100)
			{
				if(obj_cen.x-cvRound(p[0])<35)
				{
					if(obj_cen.y-cvRound(p[1])<35)
					{
						j++;
						if(j==1)
						{
							//Escrever texto na imagem
							if(!LITE)
								cvPutText(final, ball, cir_cen, &font, CV_RGB(255,0,0) );	
							j=0;
							ang.x=int(-(obj_cen.x-cen.x)/6);
							ang.y=int(-(obj_cen.y-cen.y)/6);
						}
					}
				}
			}
		}
		//no centro da imagem
		if(!LITE){
			cvLine(final, cvPoint(pyr->width/2-8, pyr->height/2), cvPoint(pyr->width/2+8, pyr->height/2), CV_RGB(0,0,128), 2, 8 ,0);
			cvLine(final, cvPoint(pyr->width/2, pyr->height/2-8), cvPoint(pyr->width/2, pyr->height/2+8), CV_RGB(0,0,128), 2, 8 ,0);

			if(obj_cen.x<=(pyr->width-8) && obj_cen.y<=(pyr->height-8))
			{
			//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);
			}
			cvShowImage("Color", final);
		}
		cvSaveImage("./final.jpg",final);
//****************************************************************************************
// Aplicação do controlo
//****************************************************************************************
		//criar una banda  morta
		if (abs(ang.x) > 5)
		{
			val[1] += (signed char)ang.x;	
		}
		if (abs(ang.y) > 5)
		{
			val[0] += (signed char)ang.y;	
		}
		if(val[1] > ang_max_pan)
			val[1] = ang_max_pan;
		if(val[1] < ang_min_pan)
			val[1] = ang_min_pan;
if(RS232){
		port.applyjoint(HEAD, PARAM_POSITION, val);
}
//****************************************************************************************
// Termina aplicação ?
//*****************************************************************************************
		getsec(sec, usec);
		fprintf(log, "%lu%06lu\n", sec, usec);
		if(!LITE){
			c = cvWaitKey(50);
		}
	}
//****************************************************************************************
// Salva parametros de cor
//****************************************************************************************
	obj1.CapnSave("./new.jpg");
	cvDestroyAllWindows();
	fclose(log);

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

int getdate(char buffer[])
{
//	char buffer[30];
	struct timeval tv;

	time_t curtime;

	gettimeofday(&tv, NULL); 
	curtime=tv.tv_sec;

	strftime(buffer,30,"%d-%m-%Y_%T",localtime(&curtime));

	return EXIT_SUCCESS;
}

int getsec(unsigned long &sec, unsigned long &usec)
{
	struct timeval tv;
	gettimeofday(&tv, NULL); 
	sec=tv.tv_sec;
	usec=tv.tv_usec;

	return EXIT_SUCCESS;
}