/***************************************************************************
 *   Copyright (C) 2007 by Mauro,,,					   *
 *   mauro.r@ua.pt							   *
 ***************************************************************************/


#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"

using namespace std;

//--------------------
// Variáveis Globais -
//--------------------
int GlobalPos;
int device=0;

// Opcoes de execucao
int calib = 0;
int rs232 = 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};

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

//------------------------
// Cabeçalhos de Funções -
//------------------------
void printHelp();
void printlog(FILE *f, char *pstr);
int gethour(char hour[]);
int getdate(char buffer[]);
int getsec(unsigned long &sec, unsigned long &usec);
int calcCenterMass(IplImage* img, CvScalar &cmpos);

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;}

int main(int argc, char *argv[])
{
//----------------------------------------------------------------------------------------
// Declaracao das variaveis
//----------------------------------------------------------------------------------------
	FILE *hsvparam, *log, *pe_log, *t_log, *pot_log;
	char c, str[50], slog[30], buffer[25], hour[10], pelog[30], tlog[30], potlog[30], img_name[14];
	int nzero, delta, cur_pos[2]={0,0}, indx = 0;
	unsigned long sec, usec, nexec = 0;
	signed char val[3]={0, 0, 0}, servos[3], pot[30];

	Message port;
	CVision obj1(Gwidth, Gheight, device, Gdepth, Gchannels);
	CvPoint obj_cen, cen, ang;
	CvScalar cmpos;

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

//------------
// "Menu"
//------------
	if(argc > 1)
	{
		for(unsigned i = 1; i < argc; i++)
		{
			switch(*argv[i])
			{
				case 'h': printHelp();
					return 0;
				case 'c': calib = 1;
					break;
				case 'w': lite = 0;
					break;
				case 's': rs232 = 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((log = fopen(slog,"w")) == NULL){
 		cout << "Could not create LOG file!" << endl;
 		return -1;
 	}
	printlog(log, "Variables and LOG created. Program started!\n");

	if(exectime){
		strcpy(tlog, "time");
		strcat(tlog, buffer);
 		if((t_log = fopen(tlog,"w")) == NULL){
 			printlog(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(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(log, "Could not create POTENCIOMETER log file!\n");
	 		poten = 0;
 		}
		fprintf(pot_log, "%% pan\ttilt\twaist\n");
	}

//----------------------------------------------------------------------------------------
// 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(log, "Could not open HSV parameters file!\n");
	}
		fclose(hsvparam);
//----------------------------------------------------------------------------------------
// Criacao 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 espaco 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);
	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){
	cvNamedWindow("Filtro",CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Filtro",580,0);
}

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(log, "Could not open RS232 Comm Port!\n");
		return -1;
	}
	printlog(log, "Comm port opened!\n");
	printlog(log, "Starting comm test!\n");
	if(port.testcom()<0)
	{
		printlog(log, "Comm test failed! Terminating!\n");
		return -1;
	}
	printlog(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(log, "Could not activate PWM!\n");
	else
		printlog(log, "PWM on!\n");

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

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

	for(unsigned i=0;i<3;i++)
	{
		val[i]=0;
	}
	val[1]=-10;
	if(port.applyjoint(HEAD, PARAM_POSITION, val) < 0)
		printlog(log, "Error sending position orders!\n");
	else
		printlog(log, "Initial position set!\n");
// 	cout << "To (0,0) Position!\n";
}
	
	while(c != 's')
	{
		if(exectime){
			getsec(sec, usec);
			fprintf(t_log, "%lu%06lu\t", sec, usec);}

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

// 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(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){
			if(calcCenterMass(filter, cmpos) != 0)
				printlog(log, "CM was not calculated\n");
		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];
			ang.x=int(-(obj_cen.x-cen.x) / 4);
			ang.y=int(-(obj_cen.y-cen.y) / 3);
			
		if(poten){
			if(port.readjoint(pot, HEAD, PARAM_POSITION) < 0)
				printlog(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];
		}
		
			if(poserr){
				fprintf(pe_log, "%d\t%d\t%d\t%d\n", obj_cen.x, cen.x, obj_cen.y, cen.y);}

			val[1] += (signed char)ang.x;	
			val[0] += (signed char)ang.y;
		}
		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);

			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(calib){
	 		cvShowImage("Color", final);}

		if(img_seq){
			sprintf(img_name, "./final%04d.jpg", indx++);
			cvSaveImage(img_name,final);}
// Aplicação do controlo
		
		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;
			if(port.applyjoint(HEAD, PARAM_POSITION, val) < 0)
				printlog(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);
			c = cvWaitKey(50);}

		if(calib){
	 		cvShowImage("H", h_filter);
 			cvShowImage("S", s_filter);
 			cvShowImage("V", v_filter);}

	}

// Salva parametros de cor
	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(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(log, "Program ended!\n");
	fclose(log);
	
	return EXIT_SUCCESS;
}

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

//---------------------
// Determinação da hora
//---------------------
int gethour(char hour[])
{
	struct timeval tv;
	time_t curtime;

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

	strftime(hour, 10, "%T", localtime(&curtime));

	return EXIT_SUCCESS;
}

//---------------------
// Determinação da data
//---------------------
int getdate(char buffer[])
{
	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;
}

//----------------------------------------
// Determinação do tempo em microssegundos
//----------------------------------------
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;
}

//---------------------------
// Cálculo do centro de massa
//---------------------------
int calcCenterMass(IplImage* img, CvScalar &cmpos)
{
	int i, j;
	CvScalar posx, posy;

	IplImage* x= cvCreateImage(cvSize(img->width, img->height), img->depth, 1);
	IplImage* y= cvCreateImage(cvSize(img->width, img->height), img->depth, 1);
	cvZero(x);
	cvZero(y);
	
	BwImage imgA(img);
	BwImage imgx(x);
	BwImage imgy(y);

	for(i = 0; i < img->height; i++)
	{
		for(j = 0; j < img->width; j++)
		{
			if(imgA[i][j] != 0)
			{
				imgx[i][j] = j;
				imgy[i][j] = i;
			}
		}
	}
	posx = cvAvg(x, img);
	cmpos.val[0] = posx.val[0];
	posy = cvAvg(y, img);
	cmpos.val[1] = posy.val[0];

	return 0;
}