/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <semaphore.h>
#include <getopt.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <sys/fcntl.h>

#include <iostream>

using namespace std;

#include "DataInterface.h"
#include "RobotViewData.h"
#include "StreamProcessor.h"
#include "WaveServer.h"

#if GRAPHICS==1
#include "gui_chokechain/ChokeChainCP.h"
#endif


//==========================================================================

const int DEF_PORT = 54321;

pthread_t SerialProcessorThread;
pthread_t SocketServerThread;
sem_t GuiDataSemaphore;

sem_t RunSemaphore;
bool Run=true;

int guiReadFd=-1;
int streamWriteFd=-1;

DataInterface * DataControl = NULL;

#if GRAPHICS == 1
ChokeChainCP * cp;
#endif

extern int ImageXSize;
extern int ImageYSize;

//==========================================================================

void usage(const char * pn) {
  cerr << "Usage: " << pn << " <opts>" << endl;
#if GRAPHICS == 1
  cerr << "       -g             :  show graphical interface" << endl;
#endif
  cerr << "       -n <hostname>  :  specify this to connect" << endl;
  cerr << "       -p <portnum>   :  this defaults to " << DEF_PORT << endl;
  cerr << "       -l <logname>   :  specify a log file" << endl;
  cerr << "       -s <log pos>   :  specify starting position in log file"
       << endl;
}

//==========================================================================

int main(int argc,char *argv[]) {

  // This is the primary data control interface
  // All debug information coming into the system goes through here
  // When there is new information, it appears here and it appears that
  // there is a method which can be used to check for it so that it can 
  // be displayed in the appropriate interface
  
  // The hostname of the dog to connect to
  const char * hostname=NULL;
  int portnum = DEF_PORT; // This is defined in WLOut.h...

  // Open a log file
  const char * logfilename=NULL;

  int log_position=-1;

#if GRAPHICS == 1
  bool with_graphics=false;
#endif

  //========================
  // Parse the command lines
  int c;
  while ((c = getopt(argc, argv, "l:gp:n:s:h")) >= 0) {
    switch (c) {
    case 'l':
      logfilename=optarg;
      break;
    case 'n':
      hostname=optarg;
      break;
    case 'p':
      portnum=atoi(optarg);
      break;
    case 'g':
#if GRAPHICS == 1
      with_graphics=true;
#else
      fprintf(stderr,"*******************************************************************\n");
      fprintf(stderr,"Graphics interface not supported, change the Makefile and recompile\n");
      fprintf(stderr,"*******************************************************************\n");
      exit(1);
#endif
      break;
    case 's':
      log_position=atoi(optarg);
      break;
    case 'h':
    default:
      usage(argv[0]);
      exit(1);
    }
  }
  
  DataControl = new DataInterface;
  
  if(sem_init(&GuiDataSemaphore,0,1)!=0) {
    perror("unable to create semaphore for GUI\n");
    exit(4);
  }

  if(sem_init(&RunSemaphore,0,1)!=0) {
    perror("unable to create semaphore for run var\n");
    exit(4);
  }

  int pipe_fds[2];
  if(pipe(pipe_fds)!=0) {
    perror("couldn't create pipe to talk to gui\n");
    exit(2);
  }
  guiReadFd     = pipe_fds[0];
  streamWriteFd = pipe_fds[1];

  if(fcntl(guiReadFd,F_SETFL,O_NONBLOCK)!=0) {
    perror("couldn't make gui read fd nonblocking\n");
    exit(3);
  }

  if(pthread_create(&SerialProcessorThread,NULL,SerialProcessor,NULL)!=0) {
    perror("error creating serial thread\n");
    exit(1);
  }

  WaveServerConfig wave_server_config;

  //StartServer(&SocketServerThread,&wave_server_config,&RunSemaphore,&Run);

  if (NULL != hostname) {
    // Don't read from a logfile.
    // Connect to the socket instead
    cout << "Starting socket server" << endl;
    StartServer2(&SocketServerThread,
		 &wave_server_config,
		 &RunSemaphore,
		 &Run,
		 hostname,
		 portnum);
  } else if (NULL != logfilename) {
    // Read from the logfile only if no host is selected
    cout << "Setting a logfile" << endl;
    DataControl->cacheLog(logfilename);
  }


#if GRAPHICS == 1
  if (with_graphics) {
    // Load up the new graphics/debugging interfaces 
    cp = new ChokeChainCP(guiReadFd,
			  MaxImageXSize,
			  MaxImageYSize,
			  log_position);
    cp->run();

    // This should be shutdown behavior only when the main graphics loop exits
    sem_wait(&RunSemaphore);
    Run = false;
    sem_post(&RunSemaphore);
  }
#endif
  void *return_val;
  if((errno=pthread_join(SocketServerThread,&return_val))!=0) {
    perror("Problem joining with socket server thread");
  }
  if((errno=pthread_join(SerialProcessorThread,&return_val))!=0) {
    perror("Problem joining with serial processor thread");
  }

  return 0;
}