/***************************************************************************
 *   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 <highgui.h>
#include <cv.h>
#include <cxcore.h>
#include <cvaux.h>

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

using namespace std;

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[])
{
//****************************************************************************************
// Declaração das variaveis
//****************************************************************************************
	FILE *fp;
	char str[50];
	int nzero, delta;
	signed char val[3]={0, 0, 0}, servos[3];
	Message port;
// 	CVision visarr[7];
// 	char* index[7];
	CVision obj1(640,480,device,IPL_DEPTH_8U,3), obj2;
	char ball[]="ball\0";
	double M00,M10,M01;
	CvPoint obj_cen, cen, ang, cir_cen;
	CvMoments moments;

	cen.x = obj1.m_pImg->width/2;
	cen.y = obj1.m_pImg->height/2;
	obj_cen = cen;
//****************************************************************************************
// 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;

//****************************************************************************************
// Criação das imagens
//****************************************************************************************
	IplImage* clone;
	//index[0]="Imagem Filtrada";
	IplImage* pyr = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 3);
	//visarr[0]->imgpc = cvCreateImage(cvSize(m_pImg->width/2,m_pImg->height/2), m_pImg->depth, 3);
	//index[1]="PyrDown da Imagem Original";
	IplImage* hsv = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 3);
	//index[2]="Imagem no espaço HSV";
	//Componentes da imagem HSV
	IplImage* h = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
	//index[3]="Componente H";
	IplImage* s = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
	//index[4]="Componente S";
	IplImage* v = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
	//index[5]="Componente V";

	IplImage *h_filter = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
	IplImage *s_filter = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
	IplImage *v_filter = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);

	IplImage* filter = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
	//index[6]="Filtro de Cor";
	IplImage* final = cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 3);
	IplImage*final_gray= cvCreateImage(cvSize(obj1.m_pImg->width/2,obj1.m_pImg->height/2), obj1.m_pImg->depth, 1);
// //****************************************************************************************
// // Criação das janelas e trackbars para ajuste fino
// //****************************************************************************************
// 	cout << "Hello, world!" << endl;
// 	cvNamedWindow("Pyr",CV_WINDOW_AUTOSIZE);
// 	cvMoveWindow("Pyr",250,0);
// 	cvNamedWindow("Color",CV_WINDOW_AUTOSIZE);
// 	cvMoveWindow("Color",580,0);
// 	cvNamedWindow("H",CV_WINDOW_AUTOSIZE);
// 	cvMoveWindow("H",250,295);
// 	cvNamedWindow("S",CV_WINDOW_AUTOSIZE);
// 	cvMoveWindow("S",580,295);
// 	cvNamedWindow("V",CV_WINDOW_AUTOSIZE);
// 	cvMoveWindow("V",910,295);
// 	
// 	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);
// 	//cout << CIRCLE << endl;

	//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);


	char c;
	while(c != 's')
	{
		obj1.Refresh();
		//clone = cvCloneImage(obj1.m_pImg);
// 	CvFont *font;
// 	cvInitFont(font, CV_FONT_HERSHEY_SIMPLEX, 1.0f, 1.0f, 0, 3, 8 );
// 	cvPutText(obj1.m_pImg, "SO UM TESTE!", cvPoint(0,25), font, CV_RGB(100,100,255));
// 		obj1.AddText("SO UM TESTE!");
// 		obj1.Display();

//****************************************************************************************
// Determinação dos filtros a usar para a cor
//****************************************************************************************
		cvZero(pyr);
		cvPyrDown(obj1.m_pImg, pyr, CV_GAUSSIAN_5x5 );

// 		cvShowImage("Pyr", pyr);
		cvZero(hsv);
		cvCvtColor(pyr, hsv, CV_BGR2HSV );
// 	cvNamedWindow("HSV",CV_WINDOW_AUTOSIZE);
// 	cvShowImage("HSV", pyr);
		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);

// 		cvShowImage("H", h_filter);
		cvZero(s_filter);
		cvInRangeS(s, cvScalar(color.Slow), cvScalar(color.Shigh), s_filter);

// 		cvShowImage("S", s_filter);
		cvZero(v_filter);
		cvInRangeS(v, cvScalar(color.Vlow), cvScalar(color.Vhigh), v_filter);	
// 		cvNamedWindow("V",CV_WINDOW_AUTOSIZE);
// 		cvShowImage("V", v_filter);
		cvZero(filter);
		cvAnd(h_filter, s_filter, filter, v_filter);
// 		cvNamedWindow("Filtro",CV_WINDOW_AUTOSIZE);
// 		cvShowImage("Filtro", filter);
		cvZero(final);
		cvCopy(pyr, final, filter);
//		visarr[1]->Display(index[1]);
		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
// 							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
// 		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);
//****************************************************************************************
// 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;
		port.applyjoint(HEAD, PARAM_POSITION, val);
//****************************************************************************************
// Termina aplicação ?
//*****************************************************************************************
		c = cvWaitKey(50);
	}
//****************************************************************************************
// Salva parametros de cor
//****************************************************************************************
	fp = fopen("hsv_param.txt","w");
	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);
	fclose(fp);
	obj1.CapnSave("./new.jpg");

//	cvDestroyAllWindows();

	return EXIT_SUCCESS;
}