/***************************************************************************
 *   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"
#include "imgwrap.h"

#define LITE 0
#define RS232 1
#define CALIB 0

using namespace std;

int getdate(char buffer[]);
int getsec(unsigned long &sec, unsigned long &usec);
int calcCenterMass(IplImage* img, CvScalar &cmpos);

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[26], buffer[21], img_name[14];
	int nzero, delta, calib = 1, cur_pos[2]={0,0}, indx = 0;
	unsigned long sec, usec;
	signed char val[3]={0, 0, 0}, servos[3];
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 3, 8 );

	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;
	CvScalar cmpos;

	ang.x=0;
	ang.y=0;
	cen.x = Gwidth/8;
	cen.y = Gheight/8;
	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='p';
//****************************************************************************************
// 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));
//		cout << "Colors Parameters read!" << endl;
	}
	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;
 		return -1;
 	}
//  	cout << "Log file created!" << 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);

// 	cout << "Images created!" << endl;
//****************************************************************************************
// 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);
#endif

#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);
#endif
	
// 	cvCreateTrackbar("Device","Pyr", &GlobalPos ,2,SetCamnode);
#if CALIB
	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);
#endif
 //	cvSetTrackbarPos("Device","Pyr",device);
 #if CALIB
 	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);
#endif

#if RS232
	//iniciar comm rs232
	if(port.initcom() < 0)
	{
		cout << "Could not open RS232 Comm Port!" << endl;
		return -1;
	}
//	cout << "Comm started!\n";
//	cout << "A iniciar teste de comm!\n";
	if(port.testcom()<0)
	{
		cout << "Erro na comunicacao|! A terminar!\n";
		return -1;
	}
// 	cout << "Teste finalizado com sucesso!\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;
	port.applyjoint(HEAD, PARAM_SPECIAL, servos);
// 	cout << "PWM on!\n";

	for(unsigned i=0;i<3;i++)
	{
		servos[i]=0;
	}
	port.applyjoint(HEAD, PARAM_VELOCITY, servos);
// 	cout << "Motors running!\n";

	for(unsigned i=0;i<3;i++)
	{
		servos[i]=0;
	}
	port.applycontrol(HEAD, PARAM_CONTROLON, servos);
	port.applycontrol(HEAD, PARAM_KI, servos);
	port.applycontrol(HEAD, PARAM_KP, servos);
	port.applycontrol(HEAD, PARAM_KD, servos);

	for(unsigned i=0;i<3;i++)
	{
		val[i]=0;
	}
	val[1]=-10;
	port.applyjoint(HEAD, PARAM_POSITION, val);
// 	cout << "To (0,0) Position!\n";
#endif
	
//	fprintf(log, "%% Obj_cen.x\tcen.x\tObj_cen.y\tcen.y\n");
	while(c != 's')
	{
		getsec(sec, usec);
		fprintf(log, "%lu%06lu\t", sec, usec);
		obj1.Refresh();
// 		cout << "captura\n";

// 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);
		
		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);
#if CALIB
 		cvShowImage("H", h_filter);
#endif

		cvZero(s_filter);
		cvInRangeS(s, cvScalar(color.Slow), cvScalar(color.Shigh), s_filter);
#if CALIB
 		cvShowImage("S", s_filter);
#endif

		cvZero(v_filter);
		cvInRangeS(v, cvScalar(color.Vlow), cvScalar(color.Vhigh), v_filter);	
 #if CALIB
 		cvShowImage("V", v_filter);
#endif


		cvZero(filter);
		cvAnd(h_filter, s_filter, filter, v_filter);
		//cout << "filtro\n";
		if(!LITE)
			cvShowImage("Filtro", filter);

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

		if(cvCountNonZero(filter) > 40)
		{
			if(calcCenterMass(filter, cmpos) != 0)
				cout << "CM was not calculated\n";
		//cout << cmpos.val[0] << "\t" << cmpos.val[1] << endl;
			obj_cen.x = (int)cmpos.val[0];
			obj_cen.y = (int)cmpos.val[1];
		//cout << "x=" << obj_cen.x << "\ty=" << obj_cen.y << endl;
			ang.x=int(-(obj_cen.x-cen.x) / 4);
//			cout << "obj_cen.x=" << obj_cen.x << "\tcen.x=" << cen.x << endl;
			fprintf(stdout, "COORDENADAS\t%d\t%d\t", obj_cen.x, cen.x);
			ang.y=int(-(obj_cen.y-cen.y) / 3);
			fprintf(stdout, "%d\t%d\n", obj_cen.y, cen.y);
		//	if (abs(ang.x) > 5)
		//	{
				val[1] += (signed char)ang.x;	
		//	}
		//	if (abs(ang.y) > 5)
		//	{
				val[0] += (signed char)ang.y;	
		//	}
// 			cout << "ctrl\n";
		}
// 		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);

			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);
			}
#endif

#if 1 //CALIB
 		cvShowImage("Color", final);
#endif
	sprintf(img_name, "./final%04d.jpg", indx++);
//	cout << img_name << endl;
//	cvSaveImage(img_name,final);
// Aplicação do controlo
// 		criar una banda  morta
if(RS232){
		if(val[1] > ang_max_pan)
			val[1] = ang_max_pan;
		if(val[1] < ang_min_pan)
			val[1] = ang_min_pan;
		if(val[0] > ang_max_tilt)
			val[1] = ang_max_tilt;
		if(val[0] < ang_min_tilt)
			val[1] = ang_min_tilt;
		//cout << "verif\n";
		port.applyjoint(HEAD, PARAM_POSITION, val);
		//cout << "apply\n";
}
// 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;
		else{
		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);}
#endif
	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;
}

int calcCenterMass(IplImage* img, CvScalar &cmpos)
{
	int i, j;
	CvScalar posx, posy;

	IplImage* x= cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);
	IplImage* y= cvCreateImage(cvSize(Gwidth/4,Gheight/4), Gdepth, 1);
	cvZero(x);
	cvZero(y);
//cout << "aux img\n";
	BwImage imgA(img);
	BwImage imgx(x);
	BwImage imgy(y);
//cout << "aux class\n";

	for(i = 0; i < Gheight/4; i++)
	{
		for(j = 0; j < Gwidth/4; j++)
		{
			if(imgA[i][j] != 0)
			{
			//cout<< "teste\n" << i << "\t" << j << endl;
				imgx[i][j] = j;
				imgy[i][j] = i;
			}
		}
	}
// 	cvNamedWindow("ImgA",CV_WINDOW_AUTOSIZE);
// 	cvShowImage("ImgA", imgx.imgp);
// 	cvWaitKey();
	posx = cvAvg(x, img);
	cmpos.val[0] = posx.val[0];
//cout << cmpos.val[0] << endl;
	posy = cvAvg(y, img);
	cmpos.val[1] = posy.val[0];

	return 0;
}