#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <sys/time.h>

#include "sick.h"
#include "logutils.h"
#include "GetOpt.h"
#include "SSocket.h"

#define LOGCHANNEL "SICKD"

// private data types
typedef struct listenerinfo listenerinfo_t;
typedef struct client client_t;
 
typedef struct listenerinfo
{
  int port;
  int listenqueue;
  pthread_t listenThread;
 
  SSocket serversocket;
};
 
typedef struct client
{
  int clientid;
  listenerinfo_t *li;
  pthread_t clientThread;
  SSocket *sock;
};

// non-exported local functions
void doUsage(char *progname);
void *listenerThreadProc(void *arg);
void *clientThreadProc(void *arg);

// Global variables
Vector<client_t*> clients;
GetOpt *opt;
pthread_mutex_t mutex;
pthread_t listenerThread;

// main entry point
int main(int argc, char *argv[])
{
  sick_t *sick;
  int data[361];

  logLevel(LOG_DEBUG);

  opt=new GetOpt();
  opt->addOptionBool('h',"help", false,"Show this");
  opt->addOptionBool('v',"verbose",true,"Enable verbose output");
  opt->addOptionBool(0,"debug",false,"Enable debugging output");

  opt->addSpacer("");

  opt->addOptionInt(0,"hz",15,"Target update rate (in Hertz)");

  opt->addOptionString('d',"device","/dev/sick","Device to connect to");
  opt->addOptionInt('b',"baud",500000,"Baud rate");

  opt->addSpacer("");

  opt->addOptionInt('p',"port",7322,"TCP port for clients");
  opt->addOptionString(0,"logfile","stdout","Log to file");
  opt->addOptionBool(0,"allowremote",false,"Allow remote connections");

  if (!opt->parse(argc, argv, true))
    {
      doUsage(argv[0]);
      return -1;
    }

  pthread_mutexattr_t mutexAttr;
  pthread_mutexattr_init(&mutexAttr);
  pthread_mutexattr_settype(&mutexAttr, PTHREAD_MUTEX_RECURSIVE_NP);
  
  pthread_mutex_init(&mutex, &mutexAttr);
 
  char *logfile=opt->getOptionString("logfile");
  if (strcmp(logfile,"stdout"))
    {
      if (!logSetOutputFile(logfile))
        {
          printf("Couldn't open log file.\n");
        }
    }
 
  if (opt->getOptionBool("verbose"))
    logLevel(LOG_VERBOSE);
                                                          
  if (opt->getOptionBool("debug"))
    {
      opt->showOptions();
      logLevel(LOG_DEBUG);
    }

  if (opt->getOptionBool("help"))
    {
      doUsage(argv[0]);
      return EXIT_SUCCESS;
    }
                                                          

  sick=sick_create();
  if (!sick_connect(sick,opt->getOptionString("device")))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't connect to scanner. Exiting.");
      perror("Error");
      return -1;
    }

  if (!sick_setBaud(sick, opt->getOptionInt("baud")))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't change baud. Exiting.");
      return -1;
    }

 // the listener thread listens on TCP ports and handles command
  // streams originating there.
  //
  listenerinfo_t *li=(listenerinfo_t*) calloc(1, sizeof(listenerinfo_t));
  li->port=opt->getOptionInt("port");
  li->listenqueue=10;

  if (li->serversocket.listen(li->port, li->listenqueue, !opt->getOptionBool("allowremote")))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Couldn't listen on port %i",li->port);
      return -1;
    }
 
  if (pthread_create (&listenerThread, NULL, listenerThreadProc, li))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create listener thread");
      return -1;
    }


  sick_continuousEnable(sick, 1);

  signal(SIGPIPE, SIG_IGN);

  log(LOG_OUTPUT,LOGCHANNEL,"Good to go!");

  char buf[4096]; // big enough for 800 data items, 721 is absolute max

  int scancount=0;
  int sentscancount=0;
  int badscans=0;

  struct timeval lastReportTime; // the last time we published a summary message
  struct timeval lastScanTime;   // the last time we sent a message to our clients

  gettimeofday(&lastScanTime, NULL);
  gettimeofday(&lastReportTime, NULL);

  float hzms=1.0/opt->getOptionInt("hz");

  while(true)
    {
      int nsamples;
      int pos=0;

      struct timeval thisTime;
      gettimeofday(&thisTime, NULL);
      
      float elapsedReportTime=(thisTime.tv_sec-lastReportTime.tv_sec)+(thisTime.tv_usec-lastReportTime.tv_usec)/1000000.0;
      
      if (elapsedReportTime>5.0)
	{
	  lastReportTime=thisTime;

	  log(LOG_VERBOSE,LOGCHANNEL,"Good: %.1f%%   IN: %.1f Hz   OUT: %.1f Hz",
	      (scancount-badscans)*100.0/scancount, scancount/elapsedReportTime, sentscancount/elapsedReportTime);
	  scancount=0;
	  badscans=0;
	  sentscancount=0;
	}

      scancount++;

      int res=sick_continuousData(sick, data, &nsamples);

      if (!res)
	{
	  badscans++;
	  log(LOG_WARN,LOGCHANNEL,"bad laser read");
	  continue;
	}

      float elapsedTime=(thisTime.tv_sec-lastScanTime.tv_sec)+(thisTime.tv_usec-lastScanTime.tv_usec)/1000000.0;

      if (elapsedTime<hzms)
	continue;
      
      lastScanTime=thisTime;
      sentscancount++;

      // construct the message
      pos+=sprintf(&buf[pos], "%4X ",nsamples);
      for (int i=0;i<nsamples;i++)
	{
	  pos+=sprintf(&buf[pos], "%4X ", data[i]);
	}
      
      pos+=sprintf(&buf[pos],"\n");
      
      pthread_mutex_lock(&mutex);
      int deleted=0;
      for (int i=0;i<clients.getSize();i++)
	{
	  client_t *c=clients.get(i);
	  
	  if (c->sock->write(buf, pos)<=0)
	    {
	      log(LOG_VERBOSE,LOGCHANNEL,"client disconnected");
	      c->sock->close();
	      delete c->sock;
	      clients.put(i, NULL);
	      free(c);
	      deleted++;
	    }
	}
      if (deleted)
	clients.deleteAll(NULL);
      
      pthread_mutex_unlock(&mutex);
    }

  printf("disabling continuous\n");
  sick_continuousEnable(sick,0);
  return 0;
}

void doUsage(char *progname)
{
  printf("usage: %s [options]\n",progname);

  opt->doUsage();
  opt->showOptions();
  printf("That's it. I know that's not helpful.\n");
}

void *listenerThreadProc(void *arg)
{
  listenerinfo_t *li=(listenerinfo_t*) arg;
 
  int ot;
  pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,&ot);
 
  while(true)
    {
      client_t *ci=(client_t*) calloc(1,sizeof(client_t));
 
      ci->li=li;
      ci->sock=li->serversocket.accept();
                                                                                                
      log(LOG_VERBOSE,LOGCHANNEL,"client connected");
      pthread_mutex_lock(&mutex);
      clients.add(ci);
      pthread_mutex_unlock(&mutex);
                                                                                                
    }
                                                                                                
  return NULL;
}