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

#include "stdafx.h"


#include "stdafx.h"
#include "WIAClientProducerSMCPP.h"
#include <vrpn_Tracker.h>
#include <tinyxml.h>

#define WIACLIENT_CONFIG_FILE "wiaclient.config"


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

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

bool InitTCPWIAClientConnection(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)  // Create Shared Memory item for WIA Client
	   return -1;

#ifdef USE_WIACLIENT
   err = InitTCPWIAClientConnection(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 timeval tbegin;
   static timeval tfin;
   long delay_sec;
   long delay_usec;
   static WIAClientLibObjects shared_data;
   int len;
   double lapse;
   double meanlapse=1.0/wiaclient_meanfrequency;
   double warninglapse = (1.0 + wiaclient_frequency_warning)*meanlapse;
   FILE *f;

   shared_data.is_wiaclient_connected = true;
    
   gettimeofday(&t0,NULL);
   if (wiaclient_produce_logfile == true)
   {
	   f = fopen(wiaclient_log_fic, "w");
	   fclose(f);
   }

   while ( wiaclient_producer_update_quit == false)
   {
	   gettimeofday(&tbegin,NULL);
	 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);
#ifdef WIACLIENT_TRACE_ON
		 printf("%ld %lf %d\n", shared_data.id, shared_data.sec, shared_data.data.jpeg_stream_len);
#endif
		 if (wiaclient_produce_logfile == true)
		 {
			 f = fopen(wiaclient_log_fic, "a");
			 fprintf(f,"%ld %lf %d\n", shared_data.id, shared_data.sec, shared_data.data.jpeg_stream_len);
			 fclose(f);;
		 }
	
		 gettimeofday(&tfin,NULL);
	     delay_sec = (tfin.tv_sec - tbegin.tv_sec);
	     delay_usec = (tfin.tv_usec - tbegin.tv_usec);
	     lapse = delay_sec + delay_usec/1000000.0F;
		 while (lapse < meanlapse)
		 {
			 Sleep(1);
			 gettimeofday(&tfin,NULL);
		     delay_sec = (tfin.tv_sec - tbegin.tv_sec);
		     delay_usec = (tfin.tv_usec - tbegin.tv_usec);
	         lapse = delay_sec + delay_usec/1000000.0F;
		 }
#ifdef WIACLIENT_TRACE_ON
		 printf("lapse: %lf, ideal: %lf\n", lapse, WIACLIENT_LAPSE_LIMIT);
#endif
		 if (lapse > warninglapse)
		 {
			 printf("WARNING!!! WIACLIENT UPDATE, frame %d, elapsed time = %lf > LIMIT=%lf\n",shared_data.id, lapse, warninglapse);
		 }
   }
   printf("--> Exiting.\n");
}

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

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

  while (key != 'q')
  {
	  if (!kbhit())
	  {
		  key = getch();
		  if (key == 't')
		  {
			  if (wiaclient_produce_logfile == true)
			  {
				  wiaclient_produce_logfile = false;
				  printf("--> Trace is set to OFF\n");
			  }
			  else
			  {
				  wiaclient_produce_logfile = true;
				  printf("--> Trace is set to ON\n");
			  }
		  }
	  }
	  
  }

  wiaclient_producer_update_quit = true;
  return 0;
}

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

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

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

int _tmain(int argc, char* argv[])
{
   char *prm1;
   int index_sm;
   pthread_t thread_wiaclientproducer;
   pthread_t thread_wiaclientproducer_clavier;
   void *ret_wiaclientproducer;

   if (argc == 1)
   {
	  //sprintf(remote_ip, "%s", DEFAULT_WIACLIENT_IP);
	  //tcp_port = DEFAULT_WIACLIENT_TCP_PORT;
	   printf("--> Loading WIAClient config file: %s\n", WIACLIENT_CONFIG_FILE);
	   if (!ReadWIAConfigFile( WIACLIENT_CONFIG_FILE))
	   {
		   return 0;
	   }
	   
   }
   else if (argc == 2)
   {
	   printf("--> Loading WIAClient config file: %s\n", (argv[1]));
	   if (!ReadWIAConfigFile( argv[1]))
	   {
		   return 0;
	   }
   }
   else
   {
	   printf("Wrong number of arguments.");
	   return 0;
   }

   printf("Initializing WIA streaming on IP %s, port %d...\n", wia_remote_ip, wia_tcp_port);
   if ((index_sm=WIAClientStartProduce( wia_tcp_port, (char *)wia_remote_ip )) == -1)
	   return 0;
   printf("Starting WIA streaming!\n");
   if (wiaclient_produce_logfile == true)
	   printf("LOGGING activity is ON, log file: %s\n", wiaclient_log_fic);
   else
	   printf("LOGGING activity is OFF\n");
   printf("Mean frequency set to %2.1f Hz, with warning if frame elapsed time greater than %2.1f %%\n", wiaclient_meanfrequency, wiaclient_frequency_warning*100.0);


  pthread_mutex_init (&wiaclientproducer_mutex, NULL);

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

  (void)pthread_join (thread_wiaclientproducer, &ret_wiaclientproducer);

   stop_shared_memory(index_sm);
   
   return 0;
}


