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

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

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

static pthread_mutex_t wifibotproducer_mutex;
static int leftSpeed=0;
static int rightSpeed=0;

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

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

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;
   static timeval t0;
   static timeval tbegin;
   static timeval tfin;
   long delay_sec;
   long delay_usec;
   double lapse;
#ifdef USE_WIFIBOT
   double timestamp;
#endif

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

   gettimeofday(&t0,NULL);
   if (wifibot_produce_logfile == true)
   {
	   f = fopen(wifibot_log_fic, "w");
	   fclose(f);
   }

   while ( wifibot_producer_update_quit == false)
   {
	   gettimeofday(&tbegin,NULL);
	   i++;


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

#ifdef USE_WIFIBOT
	   err = wifibot_device->Update(&(shared_data.data));
		if (err != WIFIBOT_ERR_NONE)
		{
			printf("*** Erreur Update\n");
			fclose(f);
			return 0;
		}
#else
	   shared_data.data.batVoltage = 200;
#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_wifibot_data_to_sm(&shared_data );
#ifdef WIFIBOT_TRACE_ON
	   printf("%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);
#endif
	   if (wifibot_produce_logfile == true)
		 {
			 f = fopen(wifibot_log_fic, "a");
	         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);
	         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 < WIFIBOT_LAPSE_TIME)
		 {
			 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 WIFIBOT_TRACE_ON
		 printf("Wifibot update: lapse: %lf, ideal: %lf\n", lapse, WIACLIENT_LAPSE_LIMIT);
#endif
	   if (lapse > WIFIBOT_LAPSE_LIMIT)
		 {
			 printf("WARNING!!! WIFIBOT UPDATE, frame %d, elapsed time = %lf > LIMIT=%lf\n",shared_data.id, lapse, WIFIBOT_LAPSE_LIMIT);
		 }
   }
   printf("--> Exiting.\n");
}

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

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

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

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[])
{
   char *prm1;
   int index_sm;
   pthread_t thread_wifibotproducer;
   pthread_t thread_wifibotproducer_clavier;
   void *ret_wifibotproducer;

   if (argc == 1)
   {
	  //sprintf(remote_ip, "%s", DEFAULT_WIACLIENT_IP);
	  //tcp_port = DEFAULT_WIACLIENT_TCP_PORT;
	   printf("--> Loading Wifibot config file: %s\n", WIFIBOT_CONFIG_FILE);
	   if (!ReadWifibotConfigFile( WIFIBOT_CONFIG_FILE))
	   {
		   return 0;
	   }
	   
   }
   else if (argc == 2)
   {
	   printf("--> Loading Wifibot config file: %s\n", (argv[1]));
	   if (!ReadWifibotConfigFile( argv[1]))
	   {
		   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);

  if (pthread_create (&thread_wifibotproducer, NULL, WifibotProducerUpdate, NULL) < 0) {
    fprintf (stderr, "pthread_create error for thread  WifibotClientProducerUpdate\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_wifibotproducer, &ret_wifibotproducer);

   stop_shared_memory(index_sm);
   
   return 0;

}

