// Projet_TER_Wifibot.cpp : définit le point d'entrée pour l'application console.
//

#include "stdafx.h"
#include "Projet_TER_Wifibot.h"
#include <ijl.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <vrpn_Tracker.h>
#include <tinyxml.h>

#define WIACLIENT_LOG_FIC "wiaclientdata.log"
#define WIFIBOT_LOG_FIC "wifibotdata.log"

using namespace cv;
using namespace std;

static pthread_mutex_t wiaclientreader_mutex;
static pthread_mutex_t wiaclientproducer_mutex;
static pthread_mutex_t wifibotreader_mutex;
static pthread_mutex_t wifibotproducer_mutex;

static bool zone_rouge_detectee=false;
static int position_croix_x; // position en x du barycentre de l'objet rouge à l'instant t
static int position_croix_y; // position en y du barycentre de l'objet rouge à l'instant t
static int leftSpeed; // Modifier cette variable pour modifier la vitesse de la roue gauche
static int rightSpeed; // Modifier cette variable pour modifier la vitesse de la roue droite

static JPEG_CORE_PROPERTIES jpeg;
static BYTE jpegtemp[300000]; // Image RGB décodée à l'instant t
static int height;
static int width;

#define WIFIBOT_CONFIG_FILE "wifibot.config"
#define WIFIBOT_LAPSE_TIME (1.0/20.0)
#define WIFIBOT_LAPSE_LIMIT (1.0/20.0)*1.1
#define WIACLIENT_CONFIG_FILE "wiaclient.config"

static bool wifibot_producer_update_quit = false;
static int wifibot_tcp_port;
static char wifibot_remote_ip[100];
static bool wifibot_produce_logfile=false;
static char wifibot_log_fic[200];

static bool wiaclient_producer_update_quit = false;
static int wia_tcp_port;
static char wia_remote_ip[100];
static bool wiaclient_produce_logfile=false;
static char wiaclient_log_fic[200];
static float wiaclient_meanfrequency=30.0;
static float wiaclient_frequency_warning = 0.1;

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

int WifibotStartRead(  )
{
	int index_sm;
	index_sm = attach_wifibot_sm();
	return index_sm;
}

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

int WIAClientStartRead(int width_in, int height_in )
{
	int index_sm;

	if ((index_sm = attach_wiaclient_sm()) < 0)
	{
		printf("*** ERROR! WIAClientStartRead() Cannot attach wiaclient_sm!\n");
	   return -1;
	}

	height = height_in;
	width = width_in;
	
	return index_sm;
}

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

void *WIAClientReader(void *args)
{
	double sec;
	unsigned long new_frame_id;
	unsigned long old_frame_id;
	FILE *f;
	static wiaclient_data new_data;
	int len_jpeg;
	char jpeg_fic[200];
	int aux;

	if (aux = ijlInit(&jpeg) != 0)
	{
		printf("IjlInit() erreur %d\n", aux);
		return 0;
	}
    jpeg.JPGFile = NULL;
	Mat image = Mat(height, width,CV_8UC3);

   old_frame_id = get_wiaclient_frame_id();
   while (1)
   {
	new_frame_id = get_wiaclient_frame_id();
	if (is_wiaclient_connected() == true && (new_frame_id != old_frame_id) && (new_frame_id != NO_VALID_FRAME))
	{
	  get_wiaclient_frame_sec( &sec);
#ifdef WIACLIENT_TRACE_ON
	  printf("num frame: %lu, Time (s): %1.3lf, (%d octets)\n", get_wiaclient_frame_id(), sec, get_jpeg_length());
#endif
	  len_jpeg = get_jpeg_length();
	  copy_jpeg_data(&new_data);
	  sprintf(jpeg_fic,"tmp/img%d.jpg", get_wiaclient_frame_id());

	  jpeg.JPGBytes= new_data.jpeg_stream_data;
	  
	  jpeg.JPGSizeBytes=len_jpeg;//+posend+2;
	
		if(( aux = ijlRead(&jpeg, IJL_JBUFF_READPARAMS))==0)
		{
			jpeg.JPGColor = IJL_YCBCR;
			// Set up the info on the desired DIB properties.
			jpeg.DIBColor = IJL_BGR;
			jpeg.DIBChannels = 3;
			jpeg.DIBWidth = jpeg.JPGWidth;
			jpeg.DIBHeight = -jpeg.JPGHeight;
			jpeg.DIBPadBytes = 0;
			jpeg.DIBBytes = jpegtemp;
			jpeg.JPGFile = jpeg_fic;
			
			// Now get the actual JPEG image data into the pixel buffer.
			if (aux = ijlRead(&jpeg, IJL_JBUFF_READWHOLEIMAGE) < 0)
			{
#ifdef WIACLIENT_TRACE_ON
				printf("IjlRead() Bis err = %d\n", aux);
#endif
			}
		}
		else
		{
			printf("IjlRead() err = %d\n", aux);
		}
		ijlWrite(&jpeg,IJL_JFILE_WRITEWHOLEIMAGE);
		image.data = jpegtemp;
		if (zone_rouge_detectee == true)
		{
		/*
		A FAIRE
		Afficher une croix à la position (position_croix_x,position_croix_y)
		*/
         circle(image, Point (position_croix_x,position_croix_y), 5,Scalar(0,255,255),1,8,0);
		}
		
        imshow("Image display", image);
		waitKey(20);
		Sleep(5);

	  old_frame_id = new_frame_id;
	}
   }
}

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

bool InitWifibotConnection(int portNo, char* ipAddress)
{
	bool res;
	wifibot_device = new Wifibot();
#ifdef WIFIBOT_TRACE_ON
	printf("Initializing TCP communication with Wifibot sensors server on IP: %s on port %d.\n", ipAddress, portNo);
#endif
	res = wifibot_device->Connect(portNo, ipAddress);
	if (res)
	{
#ifdef WIFIBOT_TRACE_ON
		printf("Connection Ok!\n");
#endif

	}
	else
	{
#ifdef WIFIBOT_TRACE_ON
		printf("Cannot connect to Wifibot with Wifibot sensors server on IP: %s on port %d.\n", ipAddress, portNo);
#endif		
	}

	return res;
}

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

int WifibotStartProduce(int tcp_port, char *remote_ip )
{
  int index_sm;
  int err;
   if ( (index_sm = create_wifibot_sm()) == -1)
	   return -1;

#ifdef USE_WIFIBOT
   err = InitWifibotConnection(tcp_port, remote_ip);
	if (err != WIFIBOT_ERR_NONE)
	{
		printf("*** Cannot connect to Wifibot server on IP %q, port=%d!\n", remote_ip, tcp_port);
		return -1;
	}
#endif

  return index_sm;
}

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

void *WifibotProducerUpdate(void *args)
{
   FILE *f;
   WifibotLibObjects shared_data;
   shared_data.is_wifibot_connected = true;
   unsigned long i=0;
   int err;

   pthread_mutex_lock (&wifibotproducer_mutex);
   shared_data.data.leftSpeed = leftSpeed;
   shared_data.data.rightSpeed = rightSpeed;
   pthread_mutex_unlock (&wifibotproducer_mutex);

   f=fopen(WIFIBOT_LOG_FIC,"w");
#ifdef USE_WIFIBOT
   double timestamp;
#endif
   timeval t0;

   gettimeofday(&t0,NULL);

   while (1)
   {
	   i++;
#ifdef USE_WIFIBOT
	   //printf("Frame %d, leftSpeed=%d, rightSpeed=%d\n", i, leftSpeed, rightSpeed);
	   pthread_mutex_lock (&wifibotproducer_mutex);
	   shared_data.data.leftSpeed = leftSpeed;
       shared_data.data.rightSpeed = rightSpeed;
	   pthread_mutex_unlock (&wifibotproducer_mutex);

	   err = wifibot_device->Update(&(shared_data.data));
		if (err != WIFIBOT_ERR_NONE)
		{
			printf("*** Erreur Update\n");
			fclose(f);
			return 0;
		}
		
		
#endif
	   shared_data.id = i;
	   gettimeofday(&(shared_data.t),NULL);
	   shared_data.sec = shared_data.t.tv_sec - t0.tv_sec + (shared_data.t.tv_usec - t0.tv_usec)/1000000.0;
	   
	   fprintf(f,"%ld %lf %d %d %d %d %d %ld %ld %d %d\n", shared_data.id, shared_data.sec, shared_data.data.batVoltage, shared_data.data.irLeft, shared_data.data.irLeft2, shared_data.data.irRight, shared_data.data.irRight2, shared_data.data.odometryLeft, shared_data.data.odometryRight, shared_data.data.leftSpeed, shared_data.data.rightSpeed);
       copy_wifibot_data_to_sm(&shared_data );
	   Sleep(20);
   }
}

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

bool InitWIAClientConnection(int portNo, char* ipAddress)
{
	bool res;
	wiaclient_device = new WIAClient();
#ifdef WIACLIENT_TRACE_ON
	printf("Initializing TCP communication with WIA server on IP: %s on port %d.\n", ipAddress, portNo);
#endif
	res = wiaclient_device->Connect(portNo, ipAddress);
	if (res)
	{
#ifdef WIACLIENT_TRACE_ON
		printf("Connection Ok!\n");
#endif

	}
	else
	{
#ifdef WIACLIENT_TRACE_ON
		printf("Cannot connect to WIA Server on IP: %s on port %d.\n", ipAddress, portNo);
#endif		
	}

	return res;
}

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

int WIAClientStartProduce(int tcp_port, char *remote_ip )
{
   int index_sm;
   int err;
   if ( (index_sm = create_wiaclient_sm()) == -1)
	   return -1;

#ifdef USE_WIACLIENT
   err = InitWIAClientConnection(tcp_port, remote_ip);
	if (err != WIACLIENT_ERR_NONE)
	{
		printf("*** Cannot connect to WIA server on IP %q, port=%d!\n", remote_ip, tcp_port);
		return -1;
	}

#endif

   if (! wiaclient_device->StartWIAStreaming( ))
   {
	   printf("*** Error !!! Cannot start WIA Streaming.\n");
	   return -1;
   }

   return index_sm;
}

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

void *WIAClientProducerUpdate(void *args)
{
	static unsigned long i=0;
#ifdef USE_WIACLIENT
   static double timestamp;
#endif
   static timeval t0;
   static WIAClientLibObjects shared_data;
   int len;
   FILE *f;

   shared_data.is_wiaclient_connected = true;
    
   gettimeofday(&t0,NULL);
   f = fopen(WIACLIENT_LOG_FIC, "w");
   while ( 1)
   {
	 i++;
#ifdef USE_WIACLIENT
#ifdef WIFIBOT_TRACE_ON
	  printf("Wifibot: Frame %d: ", i);
#endif
	  len = wiaclient_device->GetWIANextFrame(&(shared_data.data));
	  if (len < 1)
		{
			printf("*** Erreur GetWIANextFrame()\n");
			return 0;
		}
		else
		{
#ifdef WIFIBOT_TRACE_ON
			printf("%d octets\n", shared_data.data.jpeg_stream_len);
#endif
	  }
			
#endif
		 shared_data.id = i;
		 gettimeofday(&(shared_data.t),NULL);
		 shared_data.sec = shared_data.t.tv_sec - t0.tv_sec + (shared_data.t.tv_usec - t0.tv_usec)/1000000.0;
	     copy_wiaclient_data_to_sm( &shared_data);
		 fprintf(f,"%ld %lf %d\n", shared_data.id, shared_data.sec, shared_data.data.jpeg_stream_len);
		 Sleep(10);
   }
}

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

void traitement_image(Mat image)
{
	//Mat imgOriginal;
	//Mat rot_mat;
	//Point center = Point( image.cols/2, image.rows/2 );
   //rot_mat = getRotationMatrix2D( center, 180,1 );

  // warpAffine(image, imgOriginal, rot_mat, Size(image.cols, image.rows));
   
  Mat imgHSV;

   cvtColor(image, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV(Couleur Saturation Intensité)
 
  Mat imgFiltre,imgFiltre2;
  //éléments de masque fixés pour ces éléments de tableau qui sont dans la zone de délimitation spécifique à l'élément
   inRange(imgHSV, /*Scalar(0, 100, 100)*/Scalar(0, 70, 50), Scalar(10, 255, 255), imgFiltre); //Threshold the image
    inRange(imgHSV, /*Scalar(160, 100, 100)*/Scalar(170, 70, 50), Scalar(180, 255, 255), imgFiltre2);
      
	 Mat red_hue_image;
	addWeighted(imgFiltre, 1.0, imgFiltre2, 1.0, 0.0, red_hue_image);
  //morphological opening (remove small objects from the foreground)
  erode(red_hue_image, red_hue_image, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
  dilate( red_hue_image, red_hue_image, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); 

   //morphological closing (fill small holes in the foreground)
  dilate( red_hue_image, red_hue_image, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); 
  erode(red_hue_image, red_hue_image, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );

  //Calculate the moments of the thresholded image(calcule moments de la forme pixellisée ou d'un vecteur de points)
  Moments M = moments(red_hue_image);

   double dM01 = M.m01;
  double dM10 = M.m10;
  double dArea = M.m00;
 
    //calculate the position where and are the components of the centroid
   int posX = dM10 / dArea;
   int posY = dM01 / dArea;  
    // get the image data

  //100 pixel=2.646 cm
    // if the area <= 10000, I consider that the there are no object in the image and it's because of the noise, the area is not zero 
  if (dArea > 10000)
  {
	 zone_rouge_detectee=true;
     printf("Position x=% d y=%d \n",posX,posY);
	 position_croix_x = posX;
	 position_croix_y = posY;
  }
  else 
  {
	 zone_rouge_detectee=false;
	 printf("\nZone rouge NON détectée\n");
  }
   //Mat dst1,dst2;

//   resize(imgOriginal, dst1, Size(640, 480), 0, 0, INTER_CUBIC);
 //  resize(red_hue_image, dst2, Size(640, 480), 0, 0, INTER_CUBIC);
   // imshow( "Image transformée", dst2 ); //show the thresholded image
	//imshow("Image Réel", dst1); //show the original image
}

//-----------------------------------------------
// VOTRE TRAVAIL COMMENCE ICI
void *BoucleProjetTER(void *)
{
	Mat image_courante=Mat(height, width, CV_8UC3);
	int leftIR_forward;
	int rightIR_forward;
	int leftIR_back;
	int rightIR_back;
	int last_move=0;
	image_courante.data = jpegtemp; // Récupération du pointeur vers l'image courante
	pthread_mutex_lock (&wifibotproducer_mutex);
	leftSpeed = 0.0F;
	rightSpeed = 0.0F;
	pthread_mutex_unlock (&wifibotproducer_mutex);
	while ( 1)
	{
	// Boucle while(1) ou while (objet pas trop proche)
	// 1 Récupération de l'image courante -> *image_courante

	// 2 Traitement de l'image courante
	// Post: zone_rouge_detectee mis à jour
	//       position_croix_x et position_croix_y mis à jour le cas échéant (si zone_rouge_detectee vaut true)
		traitement_image(image_courante);
	// 3 Récupération de la valeur des capteurs infra-rouge
		leftIR_forward = get_wifibot_irLeft();
		leftIR_back = get_wifibot_irLeft2();
		rightIR_forward = get_wifibot_irRight();
		rightIR_back = get_wifibot_irRight2();
	// 4 Calcul de la vitesse des roue droite et gauche et mise à jours de leftSpeed et rightSpeed

		if (zone_rouge_detectee == false)
		{
			// Le robot tourne sur lui-même
			if (last_move == 0)
			{
			  leftSpeed = 30;
			  rightSpeed = -30;
			}
		}
		else
		{
			if ((position_croix_x - 160) > 20)
			{
				if (leftIR_forward < 100 && rightIR_forward < 100)
				{
				// La cible est sur la droite
				// le robot tourne sur lui-même sur la droite
				  printf("Cible sur la droite.\n");
			
				  last_move = 1;
				  leftSpeed = 40;
			      rightSpeed = -40;
				}
				else
				{
					printf("Obstacle détecté!\n");
					last_move = 0;
				    leftSpeed = 0;
			        rightSpeed = 0;
				}
			}
			else if ((position_croix_x - 160) < -20)
			{
				if (leftIR_forward < 100 && rightIR_forward < 100)
				{
				// La cible est sur la gauche
				// le robot tourne sur lui-même sur la gauche
				   printf("Cible sur la gauche.\n");
				   last_move = 2;
				   leftSpeed = -40;
			       rightSpeed = 40;
				}
				else
				{
					printf("Obstacle détecté!\n");
					last_move = 0;
				    leftSpeed = 0;
			        rightSpeed = 0;
				}
			}
			else
			{
				if (leftIR_forward < 100 && rightIR_forward < 100)
				{
					printf("Cible au centre, pas d'obstacles devant.\n");
			// Le robot avance tout droit
				  leftSpeed = 40;
			      rightSpeed = 40;
				}
				else
				{
					// On est arrivé à l'objectif!
					printf("Cible au centre, présence d'obstacles devant.\n");
					leftSpeed = 0;
			        rightSpeed = 0;
				}
			}
		}

		Sleep(30);
	}

	return 0;
}

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

bool ReadWifibotConfigFile(char *configfile)
{
   TiXmlDocument doc(configfile);
   TiXmlElement *wiaclient_xml;
   const char *text_tcp_port;
   bool ipfound=false;
   bool portfound=false;
   bool istracefound=false;
   bool islogfilefound=false;

   if(!doc.LoadFile())
	   {
		  printf("*** Failed to load %s!\n", configfile);
		  printf("error %d: %s", doc.ErrorId(), doc.ErrorDesc());
		  return false;
       }
	   TiXmlElement *root = doc.FirstChildElement();
	   if (root == NULL)
	   {
		   printf("*** Failed to load %s! root is NULL\n", configfile);
		   doc.Clear();
		   return false;
	   }
	   for (TiXmlElement *elem=root->FirstChildElement();elem!=NULL;elem=elem->NextSiblingElement() )
	   {
		   const char *elem_name=elem->Value();
		   //printf("%s\n", elem_name);
		   if (strcmp(elem_name, "Wifibot") == 0)
		   {
			   wiaclient_xml=elem;
			   break;
		   }
	   }
	   for (TiXmlElement *elem=wiaclient_xml->FirstChildElement();elem!=NULL;elem=elem->NextSiblingElement() )
	   {
		   const char *elem_name=elem->Value();
		   //printf("%s\n", elem_name);
		   if (strcmp(elem_name, "ServerTCPIP") == 0)
		   {
			   strcpy(wifibot_remote_ip, elem->GetText());
			   ipfound = true;
			   //printf("remote IP: %s\n", remote_ip);
		   }
		   else if (strcmp(elem_name, "ServerTCPPort") == 0)
		   {
			   text_tcp_port = elem->GetText();
			   wifibot_tcp_port = atoi(text_tcp_port);
			   portfound = true;
			   //printf("TCP port = %d\n", tcp_port);
		   }
		   else if (strcmp(elem_name, "Trace") == 0)
		   {
			   TiXmlAttribute* pAttrib = elem->FirstAttribute();
			   while (pAttrib)
			   {
				   
				   if (strcmp(pAttrib->Name(),"IsTraced")==0)
				   {
					   char *istracedchar;
					   istracedchar = (char *)pAttrib->Value();
					   if (atoi(istracedchar) == 1)
					   {
						   //printf("IsTraced=true\n");
						   wifibot_produce_logfile = true;
						   istracefound = true;
					   }
					   else
					   {
					       wifibot_produce_logfile = false;
						   //printf("IsTraced=false\n");
					   }
				   }
				   else if (strcmp(pAttrib->Name(),"LogFile")==0)
				   {
					   char *logfilechar;
					   logfilechar = (char *)pAttrib->Value();
					   strcpy(wifibot_log_fic, logfilechar);
					   islogfilefound = true;
					   //printf("Log file name: %s\n", wiaclient_log_fic);
				   }
				   else
				   {

				   }
				   pAttrib = pAttrib->Next();
			   }
		   }
	   }
  if (ipfound == false)
  {
	  printf("ERROR!!! ReadConfigFile() No ServerTCPIP item found in the config file.\n");
	  return false;
  }
  if (portfound == false)
  {
	  printf("ERROR!!! ReadConfigFile() No ServerTCPPort item found in the config file.\n");
	  return false;
  }
  if (islogfilefound==true && istracefound == false)
  {
	  printf("ERROR!!! ReadConfigFile() LogFile attribute found but IsTraced attribute not found or set to 0 in the config file.\n");
	  return false;
  }
  if (islogfilefound==false && istracefound == true)
  {
	  printf("ERROR!!! ReadConfigFile() LogFile attribute not found but IsTraced attribute set to 1 in the config file.\n");
	  return false;
  }
  return true;
}

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

bool ReadWIAConfigFile(char *configfile)
{
   TiXmlDocument doc(configfile);
   TiXmlElement *wiaclient_xml;
   const char *text_tcp_port;
   bool ipfound=false;
   bool portfound=false;
   bool istracefound=false;
   bool islogfilefound=false;
   bool isfrequencymeanfound=false;
   bool isfrequencywarningfound=false;

   if(!doc.LoadFile())
	   {
		  printf("*** Failed to load %s!\n", configfile);
		  printf("error %d: %s", doc.ErrorId(), doc.ErrorDesc());
		  return false;
       }
	   TiXmlElement *root = doc.FirstChildElement();
	   if (root == NULL)
	   {
		   printf("*** Failed to load %s! root is NULL\n", configfile);
		   doc.Clear();
		   return false;
	   }
	   for (TiXmlElement *elem=root->FirstChildElement();elem!=NULL;elem=elem->NextSiblingElement() )
	   {
		   const char *elem_name=elem->Value();
		   if (strcmp(elem_name, "WiaClient") == 0)
		   {
			   wiaclient_xml=elem;
			   break;
		   }
	   }
	   for (TiXmlElement *elem=wiaclient_xml->FirstChildElement();elem!=NULL;elem=elem->NextSiblingElement() )
	   {
		   const char *elem_name=elem->Value();
		   if (strcmp(elem_name, "ServerTCPIP") == 0)
		   {
			   strcpy(wia_remote_ip, elem->GetText());
			   ipfound = true;
		   }
		   else if (strcmp(elem_name, "ServerTCPPort") == 0)
		   {
			   text_tcp_port = elem->GetText();
			   wia_tcp_port = atoi(text_tcp_port);
			   portfound = true;
		   }
		   else if (strcmp(elem_name, "Trace") == 0)
		   {
			   TiXmlAttribute* pAttrib = elem->FirstAttribute();
			   while (pAttrib)
			   {
				   
				   if (strcmp(pAttrib->Name(),"IsTraced")==0)
				   {
					   char *istracedchar;
					   istracedchar = (char *)pAttrib->Value();
					   if (atoi(istracedchar) == 1)
					   {
						   wiaclient_produce_logfile = true;
						   istracefound = true;
					   }
					   else
					   {
					       wiaclient_produce_logfile = false;
					   }
				   }
				   else if (strcmp(pAttrib->Name(),"LogFile")==0)
				   {
					   char *logfilechar;
					   logfilechar = (char *)pAttrib->Value();
					   strcpy(wiaclient_log_fic, logfilechar);
					   islogfilefound = true;
				   }
				   else
				   {

				   }
				   pAttrib = pAttrib->Next();
			   }
		   }
		   else if (strcmp(elem_name, "Frequency") == 0)
		   {
			   TiXmlAttribute* pAttrib = elem->FirstAttribute();
			   while (pAttrib)
			   {
				   if (strcmp(pAttrib->Name(),"Mean")==0)
				   {
					   char *meanchar;
					   meanchar = (char *)pAttrib->Value();
					   wiaclient_meanfrequency = atof( meanchar);
					   isfrequencymeanfound = true;
				   }
				   else if (strcmp(pAttrib->Name(),"Warning")==0)
				   {
					   char *warningchar;
					   warningchar = (char *)pAttrib->Value();
					   wiaclient_frequency_warning = atof( warningchar);
					   isfrequencywarningfound = true;
				   }
				   else
				   {}
				   pAttrib = pAttrib->Next();
			   }
		   }
		   else
		   {}
		   
	   }
  if (ipfound == false)
  {
	  printf("ERROR!!! ReadConfigFile() No ServerTCPIP item found in the config file.\n");
	  return false;
  }
  if (portfound == false)
  {
	  printf("ERROR!!! ReadConfigFile() No ServerTCPPort item found in the config file.\n");
	  return false;
  }
  if (islogfilefound==true && istracefound == false)
  {
	  printf("ERROR!!! ReadConfigFile() LogFile attribute found but IsTraced attribute not found or set to 0 in the config file.\n");
	  return false;
  }
  if (islogfilefound==false && istracefound == true)
  {
	  printf("ERROR!!! ReadConfigFile() LogFile attribute not found but IsTraced attribute set to 1 in the config file.\n");
	  return false;
  }
  if (isfrequencymeanfound == false)
  {
	  printf("ERROR!!! ReadConfigFile(), Frequency, Mean attribute not found.\n");
	  return false;
  }
  return true;
}

//-----------------------------------------------
void *GestionClavierProjetTER(void *)
{
  int key=0X00;

  while (key != 'q')
  {
	  printf("leftSpeed = %d, rightSpeed = %d\n", leftSpeed, rightSpeed);
	  if (!kbhit())
	  {
		  key = getch();

		  if (key == 'r')
		  {
			  printf("*** Vitesse mise à 0 tics\n");
			  leftSpeed = 0;
			  rightSpeed = 0;
		  }
		  else if (key == 'm')
		  {
			  printf("*** Vitesse mise à 30 tics\n");
			  pthread_mutex_lock (&wifibotproducer_mutex);
			  leftSpeed = 30;
			  rightSpeed = 30;
			  pthread_mutex_unlock (&wifibotproducer_mutex);
		  }
		  else if (key == 'n')
		  {
			  printf("*** Vitesse mise à -30 tics\n");
			  pthread_mutex_lock (&wifibotproducer_mutex);
			  leftSpeed = -30;
			  rightSpeed = -30;
			  pthread_mutex_unlock (&wifibotproducer_mutex);
		  }
	  }

  }
  return 0;
}

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

void *WifibotGestionClavier(void *)
{
  int key=0X00;

  while (key != 'q')
  {
	  if (!kbhit())
	  {
		  key = getch();
		  if (key == 't')
		  {
			  if (wifibot_produce_logfile == true)
			  {
				  wifibot_produce_logfile = false;
				  printf("--> Trace is set to OFF\n");
			  }
			  else
			  {
				  wifibot_produce_logfile = true;
				  printf("--> Trace is set to ON\n");
			  }
		  }
		  else if (key == '+')
		  {
			  pthread_mutex_lock (&wifibotproducer_mutex);
	          leftSpeed ++;
              rightSpeed ++;
	          pthread_mutex_unlock (&wifibotproducer_mutex);
			  printf("--> Left speed set to %d, Right speed set to %d\n", leftSpeed, rightSpeed);
		  }
		  else if (key == '-')
		  {
			  pthread_mutex_lock (&wifibotproducer_mutex);
	          leftSpeed --;
              rightSpeed --;
	          pthread_mutex_unlock (&wifibotproducer_mutex);
			  printf("--> Left speed set to %d, Right speed set to %d\n", leftSpeed, rightSpeed);
		  }
		  else if (key == 'r')
		  {
			  pthread_mutex_lock (&wifibotproducer_mutex);
	          leftSpeed = 0;
              rightSpeed = 0;
	          pthread_mutex_unlock (&wifibotproducer_mutex);
			  printf("--> Left speed set to %d, Right speed set to %d\n", leftSpeed, rightSpeed);
		  }
		  else
		  {
		  }
	  }
	  
  }

  wifibot_producer_update_quit = true;
  return 0;
}

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

int _tmain(int argc, char* argv[])
{
   unsigned long i;
   int err;
   pthread_t thread_wifibotproducer;
   void *ret_wifibotproducer;
   pthread_t thread_wiaclientproducer;
   void *ret_wiaclientproducer;
   pthread_t thread_wiaclientreader;
   void *ret_wiaclientreader;
   pthread_t thread_projetter;
   pthread_t thread_wifibotproducer_clavier;
   void *ret_projetter;
   pthread_t thread_gestionclavier;
   void *ret_gestionclavier;
   int index_sm;
   FILE *f;
   
   if (argc == 1)
   {
	   printf("--> Loading Wifibot config file: %s\n", WIFIBOT_CONFIG_FILE);
	   if (!ReadWifibotConfigFile( WIFIBOT_CONFIG_FILE))
	   {
		   return 0;
	   }
	   printf("--> Loading WIAClient config file: %s\n", WIACLIENT_CONFIG_FILE);
	   if (!ReadWIAConfigFile( WIACLIENT_CONFIG_FILE))
	   {
		   return 0;
	   }
   }
   else if (argc == 2)
   {
	   printf("--> Loading Wifibot config file: %s\n", (argv[1]));
	   if (!ReadWifibotConfigFile( argv[1]))
	   {
		   return 0;
	   }
	   printf("--> Loading WIAClient config file: %s\n", WIACLIENT_CONFIG_FILE);
	   if (!ReadWIAConfigFile( WIACLIENT_CONFIG_FILE))
	   {
		   return 0;
	   }
   }
   else
   {
	   printf("Wrong number of arguments.");
	   return 0;
   }

     printf("Initializing Wifibot streaming on IP %s, port %d...\n", wifibot_remote_ip, wifibot_tcp_port);
   if ((index_sm=WifibotStartProduce( wifibot_tcp_port, wifibot_remote_ip )) == -1)
	   return 0;
   printf("Starting Wifibot streaming!\n");

  pthread_mutex_init (&wifibotproducer_mutex, NULL);

  printf("Initializing WIA streaming  at IP=%s, port=%d...\n", wia_remote_ip, wia_tcp_port);
  if ((index_sm=WIAClientStartProduce( wia_tcp_port, wia_remote_ip )) == -1)
	   return 0;
  printf("Starting WIA streaming!\n");

  pthread_mutex_init (&wiaclientproducer_mutex, NULL);

   if (WifibotStartRead() < 0)
	   return 0;


   pthread_mutex_init (&wifibotreader_mutex, NULL);

 if (pthread_create (&thread_wifibotproducer, NULL, WifibotProducerUpdate, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  Wifibotproducer\n");
    exit (1);
  }
 
   printf("Updating WIA streaming!\n");

  if (pthread_create (&thread_wiaclientproducer, NULL, WIAClientProducerUpdate, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  WIAClientProducerUpdate\n");
    exit (1);
  }


  Sleep(5);
  printf("*** Starting WIA Reader\n");
  if (WIAClientStartRead(320, 240) < 0)
  {
	  printf("*** WIAClientStartRead() returns an error!\n");
	   return 0;
  }
  printf("*** Updating WIA Reader\n");
 if (pthread_create (&thread_wiaclientreader, NULL, WIAClientReader, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  WIAClientReader\n");
    exit (1);
  }
  printf("*** Beginning main loop on Wifibot\n");
  if (pthread_create (&thread_projetter, NULL, BoucleProjetTER, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  BoucleProjetTER\n");
    exit (1);
  }

  if (pthread_create (&thread_gestionclavier, NULL, GestionClavierProjetTER, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  BoucleProjetTER\n");
    exit (1);
  }
 
  if (pthread_create (&thread_wifibotproducer_clavier, NULL, WifibotGestionClavier, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  WIAClientProducerUpdate\n");
    exit (1);
  }

  (void)pthread_join (thread_gestionclavier, &ret_projetter);

  return 0;
}

