/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : VcsServer.cc                                                   */
/* Author   :                                                               */
/* Project  : Onboard Deliberative Autonomy                                 */
/* Version  : 1.0                                                           */
/* Created  : 02/15/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include <unix.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h> // IO Control of socket.
#include <time.h>
#include "System.h"
#include "VcsServer.h"
#include "LayeredControlIF.h"
#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"

#define CMD_DELIMITER "|"

#define D_PRINTF if (_verbose) printf

/*
 * class VcsServer
 */ 

// structors :
VcsServer::VcsServer(Boolean sim, Boolean verbose, char *config)
  :_port(0), _socket(-1), _client(-1), 
   _verbose(verbose),
   _attributes(config), _config(config),
   _layeredControlIF(NULL), _insertNum(0) {
  if( sim )
    _sim = "-sim";
  else 
    _sim = NULL;
  
  // Create the log file 
  char logName[200];
  char *logdir = getenv("AUV_LOG_DIR");
  
  for(int fi=0; ;++fi) {
    sprintf(logName, "%s/VcsServer.log.%d", logdir, fi);
    if( NULL==(_log=fopen(logName, "r")) ) {
      break; // This one appears to be a good candidate 
    } else {
      // Already existing => try next one
      fclose(_log);
    }
  }
  if( NULL==(_log=fopen(logName, "w")) ) {
    _log = stdout;
    log("VcsServer: Unable to create \"%s\" using stdout instead", logName);
  } 
  long now;
  
  time(&now);
  log("VcsServer started at %s\n", ctime(&now));     
}

VcsServer::~VcsServer() {
  log("VcsServer : bye !\n");
  if( _client>0 ) 
    close(_client);
  if( _socket>0 )
    close(_socket);
  if( _layeredControlIF )
    delete _layeredControlIF;
}

// interfaces :

int VcsServer::init() {
  char configFile[100];
  
  strcpy(configFile, System::configurationFile(_config));
  createCfgAttributes();
  System::copyToLogDir(configFile);
  // parse config file
  AttributeParser::parse(configFile, &_attributes);
  if( _port<2000 ) {
    log("VcsServer: Port %d is below 2000, defaulting to %d\n", 
	_port, DEFAULT_PORT);
    _port = DEFAULT_PORT;
  }
  log("VcsServer - configuration:\n"
      "\tPORT=%d\n"
      "\tAMC_HUP=%ds\n"
      "\tLC_NEW_BEHAVIOR_TIMEOUT=%ds", _port, _hupTime, _lcTOTime);
  // create the socket server 
  bzero((char *)&_cliaddr, sizeof(_cliaddr));
  bzero((char *)&_servaddr, sizeof(_servaddr));

  _servaddr.sin_family      = AF_INET;
  _servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
  _servaddr.sin_port        = htons((short)_port);
  
  _socket=socket(AF_INET, SOCK_STREAM, 0);
  if( _socket<0 ) {
    log("VcsServer: socket server init failed: %s", strerror(errno));
    return _socket;
  }
  
  // Fix things so that we can restart VcsServer right away
  // without having to wait until the system clears the socket
  //
  int opt = 1;
  if( setsockopt(_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof (opt))<0 ) {
    log("VcsServer: setsockopt failed: %s\n", strerror(errno));
    return -1;
  }
  // bind the socket server
  if( bind(_socket, (struct sockaddr *)&_servaddr, sizeof(_servaddr))<0 ) {
    log("VcsServer: failed to bind socket: %s\n", strerror(errno));
  }
  return 0;
}
 
void VcsServer::run() {
  char *msg=NULL; 
  char *body;

  if( acceptClient()<0 ) {
    log("VcsServer: unable to initate connection with TREX => exit");
    exit(1);
  } 
  // 1) Get the "init|..." message
  do {
    if( NULL!=msg ) {
      log("VcsServer: \"%s\" is not an init\n", msg);
      delete[] msg;
    }
    
    msg = recvString();
    if( NULL==msg ) {
      log("VcsServer: socket error during init\n");
      exit(1);
    }
    body = strstr(msg, CMD_DELIMITER);
    if( NULL!=body ) {
      *body = '\0';
      ++body;
    }
  } while( NULL==body || 0!=strcasecmp(msg, "init") );
  
  if( initMission(body)<0 ) {
    log("VcsServer: failed to start the mission.");
    exit(1);
  }
  delete[] msg;
  // 2) Get the "start" message
  log("Waiting for start message from TREX");
  msg = NULL;
  do {
    if( NULL!=msg ) {
      log("VcsServer: \"%s\" is not start\n", msg);
      delete[] msg;
    }
    msg = recvString();
    if( _layeredControlIF->abortingMission() ) {
      log("VcsServer: mission aborted (see syslog for details).\n");
      close(_client);
      _client = -1;
      return;
    }
  } while( NULL==msg || 0!=strcasecmp(msg, "start") );
  struct timespec mtime;
  double start, tick;

  // Set the initial time (~TREX initial tick)
  clock_gettime(CLOCK_REALTIME, &mtime);
  start = mtime.tv_sec+(mtime.tv_nsec/1e9);
  log("VcsServer TREX mission started !\n");

  delete[] msg;
  msg = NULL;
  // 3) Mission started
  Boolean done = False;
  while( !done ) {
    msg = recvString();
    // Compute the new date
    clock_gettime(CLOCK_REALTIME, &mtime);
    tick =  mtime.tv_sec+(mtime.tv_nsec/1e9)-start;

    if( NULL==msg ) {
      // reception failed
      switch( errno ) {
      case EAGAIN:
#if 0 // this is the same value as EAGAIN
      case EWOULDBLOCK:
#endif
	log("VcsServer[%.2f] no message after time out - abort mission !\n", tick);
	_layeredControlIF->abortMission();
	done = True;
	break;
      case ENOMSG:
	log("VcsServer[%.2f] lost connection\n", tick);
	done = True;
	break;
      default:
	log("VcsServer[%.2f] unknown socket error: %s\n", tick, strerror(errno));
	done = True;
	break;
      }
    } else {
      if( 0==strcasecmp(msg, "exit") ) {
	log("VcsServer[%.2f] received exit from TREX.\n", tick);
	_layeredControlIF->abortMission();
	done = True;
      } else if( 0==strcasecmp(msg, "ping") ) {
	// log("VcsServer[%.2d] ping\n", tick);
      } else 
	handleAmcRequest(tick, msg);
      delete[] msg;
    }
  }
  log("VcsServer: Closing connection to client\n"); 
  close(_client);
  _client = -1;
}

// Socket management 

int VcsServer::acceptClient() {
  int clilen = sizeof(_cliaddr);
  
  log("VcsServer: Waiting for new client ...\n");
  if( listen(_socket, 1)<0 ) {
    log("VcsServer: error listening to socket: %s\n", strerror(errno));
    return -1;
  }
  // Accept the new connection
  _client = accept(_socket, (struct sockaddr *)&_cliaddr, &clilen);
  if( _client<0 ) {
    log("VcsServer: error durinf client accept: %s\n", strerror(errno));
    return -1;
  }
  log("VcsServer: Connected to client on %s\n", inet_ntoa(_cliaddr.sin_addr));

  // Set the time out on message recv
  if( _hupTime>0 ) {
    struct timeval time_out;
    time_out.tv_sec = _hupTime;
    time_out.tv_usec = 0;
    if( setsockopt(_client, SOL_SOCKET, SO_RCVTIMEO, &time_out, sizeof(time_out))<0 ) {
      log("VcsServer: Failed to set socket time out: %s\n", strerror(errno));
      return -1;
    }
    log("VcsServer: socket time out is set to %ds", _hupTime);
  } else {
    log("VcsServer: no time out on socket.\n");
  }
  return 0;
}

///////////
// recvString()
//
// receive a string message using Ada/Pascal string format
//
// Instead of being a null terminated string the message contains
// a uint32_t giving the length of the string and the corresponding
// number of characters.
//
// The uint32_t is received taking into account endianess (using nothl function)
//
// returning NULL means that I have received an  empty string (len==0)
//
// Note : this function allocaters memory for the string you need to take care of its deallocation using delete[]
char *VcsServer::recvString() {
  uint32_t len;
  char *res; 
  Boolean eagain = False;
  size_t toRead = sizeof(uint32_t);
  char *buf = (char *)&len;
  int ret;

  do {
    ret = recv(_client, buf, toRead, 0);
    if( ret<0 ) {
      if( EAGAIN==errno && !eagain ) {
	eagain = True;
      } else {
	log("VcsServer::recvString - socket error %d %s\n", errno, strerror(errno));
	return NULL;
      }
    } else if( 0==ret ) {
      log("VcsServer::recvString - socket closed by TREX ?\n");
      return NULL;
    } else {
      toRead -= ret;
      buf += ret;
      if( toRead>0 ) {
	if( eagain ) {
	  log("VcsServer::recvString - timed out\n");
	  return NULL;
	}
	eagain = True;
      }
    }
  } while( toRead>0 );
  
  len = ntohl(len);
  if( len>=0 ) {
    buf = res = new char[len+1];
    res[len] = '\0';
    // This loop gets len bytes to build the string
    while( len>0 ) {
      ret = recv(_client, buf, len, 0);
      if( ret<0 ) {
	if( EAGAIN==errno && !eagain )
	  eagain = True;
	else {
	  log("VcsServer::recvString - socket error %s\n", strerror(errno));
	  delete[] res;
	  return NULL;
	}
      } else if( 0==ret ) {
	log("VcsServer::recvString - socket closed by TREX ?\n");
	delete[] res;
	return NULL;
      } else {
	len -= ret;
	buf += ret;
	if( len>0 ) {
	  if( eagain ) {
	    log("VcsServer::recvString - timed out\n");
	    delete[] res;
	    return NULL;
	  }
	  eagain = True;
	}
      }
    }
    return res;
  } else
    // If message length is <0 return NULL
    return NULL;
}


// configuration management : 

void VcsServer::createCfgAttributes() {
  _attributes.add( new IntegerAttribute  ("PORT",
                   "Listening port for the AMC", &_port, DEFAULT_PORT ) );
  
  _attributes.add( new IntegerAttribute  ("AMC_HUP",
		   "Timeout (seconds) at which we figure AMC has hung-up",
                   &_hupTime, DEFAULT_HUP ) );

  _attributes.add( new IntegerAttribute  ("LC_NEW_BEHAVIOR_TIMEOUT",
                   "Timeout (seconds) for new behavior before we abort",
                   &_lcTOTime, 20 ) );
}

// supervisor execution management 

int VcsServer::initMission(char const *initialStack) {
  log("VcsServer:initMission\n%s\n", initialStack);
  
  char *dir = getenv("AUV_PLAN_DIR");
  char mfile[200];
  FILE *plan;
  
  // Create the mission file
  sprintf(mfile, "%s/AmcPlan.cfg", dir);
  if( 0!=writeAndVerify(initialStack, mfile) )
    return -1;
  // Run the supervisor
  int pid = fork();
  if( 0==pid ) {
    log("VcsServer: starting supervisor\n");
    execlp("supervisor", "supervisor", "-plan", "AmcPlan.cfg", 
	   "-dyno", _sim, NULL);
    log("VcsServer: supervisor finished\n");
    exit(1);
  } else {
    System::milliSleep(200); // wait a little to let LayeredControl start
    if( openLC()<0 ) {
      log("VcsServer: Failed to connect to LayereControl\n");
      return -1;
    }
  }
  return 0;
}

// LayeredControl 

int VcsServer::openLC() {
  try {
    _layeredControlIF = new LayeredControlIF("layeredControl");
    log("VcsServer: connected to Layered Control.\n"
	"\tSetting LC time out to %d\n", _lcTOTime);
    _layeredControlIF->setNewBehaviorTimeoutLimit(_lcTOTime);
    return 0;
  } catch(Exception e) {
    log("VcsServer: Failed to connect to LayeredControl: %s", e.msg);
    return -1;
  }
}

void VcsServer::handleAmcRequest(double tick, char const *msg) {
  char *body = strstr(msg , CMD_DELIMITER);
  int ret;
  
  if( NULL==body ) {
    log("VcsServer[%.2f] bad command format : \"%s\"\n", tick, msg);
    return;
  }
  *body = '\0';
  ++body;
  
  if( !strcasecmp(msg, "append") ) {
    log("VcsServer[%.2f] appending behaviors\n%s", tick, body);
    ret = appendBehaviors(body);
  } else if( !strcasecmp(msg, "delete") ) {
    log("VcsServer[%.2f] deleting behavior %s\n", tick , body);
    ret = deleteBehavior(body);
  } else if( !strcasecmp(msg, "insert") ) {
    log("VcsServer[%.2f] inserting behaviors\n%s", tick, body);
    ret = insertBehaviors(body);
  } else {
    log("VcsServer[%.2f] Unknown command \"%s|%s\"\n", tick, msg, body);
    return;
  }
  if( 0!=ret ) {
    log("VcsServer[%.2f] operation %s appeared to fail with body \"%s\"\n",
	tick, msg, body);
  }
}

int VcsServer::writeAndVerify(const char* behaviors, const char *file) {
  long id = -1;
  sscanf(behaviors, "id = %d", &id);
  
  // Write behaviors to disk
  //
  FILE *bf = fopen(file, "w");  // open for writing
  if( NULL!=bf ) {
    fwrite(behaviors, strlen(behaviors), 1, bf);
    fwrite("\n", 1, 1, bf);
    //    fflush(bf); // No need to fflush ... close will do it
    fclose(bf);
  } else {
    log("VcsServer - could not open %s : %s", file, strerror(errno));
    return -1;
  }
 
  //Skip checkplan for now
#if  0   
  // Run checkplan on them to verify
  //
  char cpcom[300];
  sprintf(cpcom, "checkplan %s", file);
  if (0 != system(cpcom)) {
    log("VcsServer:: - %s failed", cpcom);
    return -1;
  }
#endif 
  return 0;
}

int VcsServer::insertBehaviors(const char* behaviors) {
  static char* dir = NULL;
  LayeredControlIF::BehaviorFilename bfilename;
  
  if( NULL==dir )
    dir = getenv("AUV_PLAN_DIR");

  sprintf(bfilename, "%s/insert%d.cfg", dir, _insertNum++);
  if (0 != writeAndVerify(behaviors, bfilename))
    return -1;
    
  if (_layeredControlIF &&
      LayeredControlIF::Ok == _layeredControlIF->insertBehaviors(bfilename, 0))
    return 0;
  else {
    long id = -1;
    sscanf(behaviors, "id = %d", &id);
    log("VcsServer::insertBehaviors - failed to insert behavior %d\n", id);
    return -1;
  }
}

int VcsServer::appendBehaviors(const char* behaviors) {

  // Write behaviors to a file and invoke LC to append them
  // Place the file in the log directory
  //
  char* dir = getenv("AUV_PLAN_DIR");
  LayeredControlIF::BehaviorFilename bfilename;
  
  // Create mission file to parse
  //
  sprintf(bfilename, "%s/append%d.cfg", dir, _insertNum++);
  if (0 != writeAndVerify(behaviors, bfilename))
    return -1;
    
  if (_layeredControlIF &&
      LayeredControlIF::Ok == _layeredControlIF->appendBehaviors(bfilename))
    return 0;
  else {
    long id = -1;
    sscanf(behaviors, "id = %d", &id);
    log("VcsServer::appendBehaviors - failed to append behavior %d\n", id);
    return -1;
  }
}


int VcsServer::deleteBehavior(const char* id) {
  long n_id = atoi(id);
  
  if (_layeredControlIF &&
      LayeredControlIF::Ok == _layeredControlIF->deleteBehavior(n_id))
    return 0;
  else {
    log("VcsServer::deleteBehavior failed to delete behavior %d\n", n_id);
    return -1;
  }
}


// Log function
//
void VcsServer::log(const char *msg, ...)
{
  va_list args;
  struct timespec now;
  clock_gettime(CLOCK_REALTIME, &now);
  double timenow = now.tv_sec + (now.tv_nsec / 1e9) - 1178100000L;

  if (_log) {
    fprintf(_log, "%.3lf, ", timenow);
    va_start(args, msg);
    vfprintf(_log, msg, args);
    fprintf(_log, "\n");
    fflush(_log);
    va_end(args);
  }

  va_start(args, msg);
  vfprintf(stdout, msg, args);
  fprintf(stdout, "\n");
  va_end(args);
}