#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <wait.h>
#include <pthread.h>
#include <unistd.h>
#include <netinet/tcp.h>
#include <sys/ioctl.h>
#include <signal.h>
#include <errno.h>
#include <stdarg.h>
#include <ctype.h>
#include <sys/time.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sched.h>
#include <sys/mman.h>

#include "orcd.h"
#include "SSocket.h"
#include "serial.h"
#include "ioutils.h"
#include "logutils.h"
#include "timespec.h"
#include "orcutils.h"
#include "shell.h"
#include "ftdi.h"

#define LOGCHANNEL "ORCD"

void doUsage(orcd_t *orcd, char *progname);
void *consoleWriterThreadProc(void *arg);
void *connectThreadProc(void *arg);

extern void execd(orcd_t *orcd);

int main(int argc, char *argv[])
{
  //***********************
  // Create the orcd object
  //***********************
  orcd_t *orcd=(orcd_t*) calloc(1, sizeof(orcd_t));
  orcd->serialconsecutivetimeouts=0;
  orcd->padOldUpDown=-1;
  orcd->padOldLeftRight=-1;

  orcd->opt=new GetOpt();

  // handle command line options
  orcd->opt->addOptionBool('h',"help",false,
			   "Show this help");
  orcd->opt->addOptionBool('v',"verbose", false,
			   "Enable verbose output");
  orcd->opt->addOptionBool(0,"debug", false,
			   "Enable debugging options");

  orcd->opt->addSpacer("");

  orcd->opt->addOptionString('d',"device","/dev/orc",
			     "Serial device to use");
  orcd->opt->addOptionInt(0,"baud",115200,
			  "Baud rate");

  orcd->opt->addOptionBool(0, "ftdi", true, 
			   "Use special ftdi driver optimizations");

  orcd->opt->addSpacer("");

  orcd->opt->addOptionInt(0,"timeoutms",100,
			  "Timeout (in ms)");

  orcd->opt->addOptionInt(0,"pollms",20,
			  "Poll interval (in ms)");

  orcd->opt->addOptionInt(0,"cmdport",7320,
			  "TCP port for command clients");
  orcd->opt->addOptionInt(0,"asyncport",7321,
			  "TCP port for async clients");

  orcd->opt->addSpacer("Advanced Options");

  orcd->opt->addOptionBool(0,"realtime", false,
			   "Try to acquire realtime priority (requires root)");

  orcd->opt->addOptionBool(0,"console", true,
			   "Show program output on console");

  orcd->opt->addOptionBool(0,"shell", true,
			   "Enable shell");
  orcd->opt->addOptionBool(0,"status", true,
			   "Display status bar");
  orcd->opt->addOptionBool(0,"setserial", true,
			   "Try to set low_latency");
  orcd->opt->addOptionString('c',"config","/etc/orcd.conf",
			     "Specify configuration file");
  orcd->opt->addOptionBool(0,"allowremote",true,
			   "Allow connections from other machines");
  orcd->opt->addOptionString(0,"logfile","stdout",
			     "Log file location");

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

  
  if (pipe(orcd->consolepipe))
    {
      perror("Couldn't create pipes");
      return -1;
    }

  // fork execd. Do not move any pthreads stuff before this operation!
  if (pipe(orcd->execpipe))
    {
      perror("Couldn't create pipes");
      return -1;
    }

  pid_t pid=fork();
  if (pid<0)
    {
      perror("Couldn't fork for exec");
      return -1;
    }
  if (pid==0)
    {
      execd(orcd);
      return 0;
    }

  orcd->serialtimeoutms=orcd->opt->getOptionInt("timeoutms");

  char *logfile=orcd->opt->getOptionString("logfile");
  if (strcmp(logfile,"stdout"))
    {
      if (!logSetOutputFile(logfile))
	{
	  printf("Couldn't open log file %s.\n",logfile);
	  return 0;
	}
    }

  orcd->config=readConfigFile(orcd->opt->getOptionString("config"));
  if (orcd->config==NULL)
    {
      orcd->opt->setOptionBool("shell", false);
    }

  if (orcd->opt->getOptionBool("verbose"))
    logLevel(LOG_VERBOSE);
  
  if (orcd->opt->getOptionBool("debug"))
    {
      // debug implies verbose.
      orcd->opt->setOptionBool("verbose", true);

      orcd->opt->showOptions();
      logLevel(LOG_DEBUG);

      if (orcd->config!=NULL)
	{
	  printf("Config file: \n\n");
	  for (int i=0;i<orcd->config->getSize();i++)
	    {
	      Option *o=orcd->config->get(i);
	      printf("%20s\t=\t%s\t%i\n",o->label, o->action, o->flags);
	    }
	}
    }

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

  if (orcd->opt->getOptionBool("realtime"))
    {
      struct sched_param schedparam;
      
      if (sched_getparam(0, &schedparam))
	perror("Couldn't get scheduler parameters\n");
      schedparam.sched_priority=10;
      if (sched_setscheduler(0, SCHED_RR, &schedparam))
	perror("Couldn't set scheduler parameters\n");

      //      if (mlockall(MCL_CURRENT | MCL_FUTURE))
      //	perror("Couldn't lock down our pages");
    }

  orcd->pb=packetbuffer_create(10,ORC_MAX_PACKETSIZE);

  orcd->bytecount=0;
  orcd->transactionNextID=1; // avoid ID=0

  orcd->transactionsPending=0;

  orcd->cmdclients=0;
  orcd->asyncclients=0;

  // Create our Mutexes
  pthread_mutexattr_t mutexAttr;
  pthread_mutexattr_init(&mutexAttr);
  pthread_mutexattr_settype(&mutexAttr, PTHREAD_MUTEX_RECURSIVE_NP);

  pthread_mutex_init(&orcd->serialwriteMutex, &mutexAttr);
  pthread_mutex_init(&orcd->scoreboardMutex, &mutexAttr);
  pthread_mutex_init(&orcd->bannerMutex, &mutexAttr);
  pthread_mutex_init(&orcd->connectionCountMutex, &mutexAttr);
  pthread_mutex_init(&orcd->padeventMutex, &mutexAttr);

  // Create our condition variables
  pthread_condattr_t condAttr;
  pthread_condattr_init(&condAttr);

  pthread_cond_init(&orcd->padeventCond, &condAttr);
  pthread_cond_init(&orcd->scoreboardCond, &condAttr);

  // initialize scoreboard
  for (int i=0;i<ORC_MAX_TRANSID;i++)
    {
      orcd->transactionScoreboard[i].response=NULL;
      orcd->transactionScoreboard[i].responselen=0;
      orcd->transactionScoreboard[i].status=ORC_TRANS_FREE;
      pthread_mutex_init(&orcd->transactionScoreboard[i].mutex, &mutexAttr);
      pthread_cond_init(&orcd->transactionScoreboard[i].cond, &condAttr);
    }

  orcd->serialfd=-1;
  orcd->serialfd_connected=0;

  //*************************
  //Create background threads
  //*************************
  pthread_t newthread;

  pthread_attr_t threadAttr;
  pthread_attr_init(&threadAttr);
  pthread_attr_setstacksize(&threadAttr, PTHREAD_STACK_MIN + 32768);

  // Connect Thread
  if (pthread_create(&newthread, &threadAttr, connectThreadProc, orcd))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create connect thread");
      crashQuit(orcd);
    }  

  // COMMAND LISTENER
  threadinfo_t *cmdti=(threadinfo_t*) calloc(sizeof(threadinfo_t),1);
  cmdti->orcd=orcd;
  cmdti->sock=new SSocket();

  if (cmdti->sock->listen(orcd->opt->getOptionInt("cmdport"), 
			  10, !orcd->opt->getOptionBool("allowremote")))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Couldn't listen on command port");
      crashQuit(orcd);
    }

  if (pthread_create(&newthread, &threadAttr, commandListenerThreadProc, cmdti))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create cmd listener thread");
      crashQuit(orcd);
    }

  // ASYNC LISTENER
  threadinfo_t *asyncti=(threadinfo_t*) calloc(sizeof(threadinfo_t),1);
  asyncti->orcd=orcd;
  asyncti->sock=new SSocket();

  if (asyncti->sock->listen(orcd->opt->getOptionInt("asyncport"), 
			    10, !orcd->opt->getOptionBool("allowremote")))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Couldn't listen on async port");
      crashQuit(orcd);
    }

  if (pthread_create(&newthread, &threadAttr, asyncListenerThreadProc, asyncti))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create async listener thread");
      crashQuit(orcd);
    }

  // SERIAL READER
  if (pthread_create (&newthread, &threadAttr, serialReaderThreadProc, orcd))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create serial reader thread.");
      crashQuit(orcd);
    }

  for (int i=0;i<1;i++)
    {
      // Asynchronous poller
      if (pthread_create(&newthread, &threadAttr, asyncPollerThreadProc, orcd))
	{
	  logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create serial poller thread.");
	  crashQuit(orcd);
	}
      
      if (pthread_create(&newthread, &threadAttr, asyncPollerThreadProc2, orcd))
	{
	  logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create serial poller thread.");
	  crashQuit(orcd);
	}
    }

  // Banner redraw thread
  if (pthread_create(&newthread, &threadAttr, bannerThreadProc, orcd))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create banner thread.");
      crashQuit(orcd);
    }

  // Shell
  if (orcd->opt->getOptionBool("shell"))
    {
      if (pthread_create(&newthread, &threadAttr, shellThreadProc, orcd))
	{
	  logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create shell thread.");
	  crashQuit(orcd);
	}
    }

  // Console Writer
  if (pthread_create(&newthread, &threadAttr, consoleWriterThreadProc, orcd))
    {
      logerrno(LOG_ERROR,LOGCHANNEL,"Unable to create console writer thread.");
      crashQuit(orcd);
    }

  //*****************************************
  //We're done. Wait until we're told to quit.
  //******************************************

  //  lcdFill(orcd,0x55,0xaa);
  //  lcdPrint(orcd, 0, 0, 'A', "hello, ed");

  while(true)
    {
      //getMasterData(orcd);
      //      lcdPrint(orcd, 8, 16, 'A', "count: %i",i++);
      usleep(450000);
    }

  return EXIT_SUCCESS;
}

void crashQuit(orcd_t *orcd)
{
  if (orcd->listenerThread!=0)
    pthread_cancel(orcd->listenerThread);
  if (orcd->serialReaderThread!=0)
    pthread_cancel(orcd->serialReaderThread);

  exit(EXIT_FAILURE);
}

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

  orcd->opt->doUsage();
  printf("\n");
}

int padPoll(orcd_t *orcd)
{
  int v;

  pthread_mutex_lock(&orcd->padeventMutex);
  v=orcd->padEvent;

  // consume any up/down/right/left events, but preserve button status
  orcd->padEvent=orcd->padEvent&0x7; 
  
  pthread_mutex_unlock(&orcd->padeventMutex);

  return v;
}

// wait for a button to be pressed. Actually waits for everything to
// be released before waiting for an event.
int padGets(orcd_t *orcd)
{
  int v;

  pthread_mutex_lock(&orcd->padeventMutex);
  while ((orcd->padEvent&ORC_PAD_BUTTONS)!=0)
    {
      pthread_cond_wait(&orcd->padeventCond, &orcd->padeventMutex);
    }

  while (orcd->padEvent==0)
    {
      pthread_cond_wait(&orcd->padeventCond, &orcd->padeventMutex);
    }
  v=orcd->padEvent;
  
  // consume any up/down/right/left events, but preserve button status
  orcd->padEvent=orcd->padEvent&0x7; 

  pthread_mutex_unlock(&orcd->padeventMutex);

  return v;
}

int reconnect(orcd_t *orcd)
{
  int newfd=-1;
  // connect to the serial port
  char *devpath=orcd->opt->getOptionString("device");
  if ((newfd = serial_open(devpath))==-1)
    {
      log(LOG_ERROR,LOGCHANNEL,"Failed to connect to serial port (%s): %s", devpath, strerror(errno));
      return EXIT_FAILURE;
    }

  log(LOG_DEBUG, LOGCHANNEL, "just allocated fd = %i\n", newfd);

  if (orcd->opt->getOptionBool("setserial"))
    {
      char buf[1024];
      snprintf(buf, 1024, "setserial %s low_latency spd_shi", devpath);
      int res=system(buf);
      if (res!=0)
	{
	  log(LOG_ERROR,LOGCHANNEL,"Error executing %s",buf);
	  log(LOG_ERROR,LOGCHANNEL,"setserial returned: %i %i",res,WEXITSTATUS(res));
	  return -1;
	}
    }
  
  if (serial_setbaud(newfd, orcd->opt->getOptionInt("baud")))
    {
      log(LOG_ERROR,LOGCHANNEL,"Couldn't set baud rate");
      return EXIT_FAILURE;
    }

  // poke the ftdi driver, if applicable.
  if (orcd->opt->getOptionBool("ftdi"))
    {
      // avoid race condition on startup...
      usleep(2000);

      int res=ftdi(orcd);

      if (res)
	log(LOG_ERROR,LOGCHANNEL,"FTDI setup FAILED!");
      else
	log(LOG_VERBOSE,LOGCHANNEL,"FTDI setup succeeded.");
    }

  orcd->serialfd = newfd;

  return 0;
}

#define CONSOLEBUFSIZE 16

void *consoleWriterThreadProc(void *arg)
{
  orcd_t *orcd=(orcd_t*) arg;
  char buffer[CONSOLEBUFSIZE];

  int ot;
  pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,&ot);
  signal(SIGPIPE, SIG_IGN);

  FILE *f = fdopen(orcd->consolepipe[0], "r");

  while (1)
    {
      fgets(buffer, CONSOLEBUFSIZE, f);
      if (!orcd->consoleDisabled)
	lcdConsole(orcd, buffer);
    }
}


void *connectThreadProc(void *arg)
{
  orcd_t *orcd=(orcd_t*) arg;

  int ot;
  pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,&ot);
  signal(SIGPIPE, SIG_IGN);

  while(1)
    {
      if (!orcd->serialfd_connected)
	{
	  // close the old fd, if it's open.
	  if (orcd->serialfd!=-1)
	    {
	      log(LOG_DEBUG, LOGCHANNEL,"Closing fd %i",orcd->serialfd);
	      close(orcd->serialfd);
	      orcd->serialfd=-1;
	    }

	  log(LOG_OUTPUT, LOGCHANNEL, "Attempting to reconnect...");
	  int res=reconnect(orcd);
	  if (res)
	    log(LOG_OUTPUT, LOGCHANNEL, "reconnect failed.");
	  else
	    {
	      log(LOG_OUTPUT,LOGCHANNEL, "reconnected (i.e. port opened)");
	      orcd->serialfd_connected=1;
	    }
	}

      sleep(1);
    }

  return NULL;
}