/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : AmcAgent.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 "System.h"
#include "AmcAgent.h"
#include "AmcMessage.h"
#include "LayeredControlIF.h"
#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"

#define CMD_DELIMITER "|"
#define NACK_CMD "FAILED"

#define D_PRINTF if (_verbose) printf

AmcAgent::AmcAgent(Boolean sim, Boolean verbose, char* config)
   : _period(200), _config(config), _port(0), _socket(-1),
      _attributes(config), _insertNum(0), _appendNum(0), _verbose(verbose),
      _layeredControlIF(0), _missionStarted(False), _sdelta(0)
{
  if (sim)
    _sim = "-sim";
  else
    _sim = 0;

  // Create a log file for this instance
  //
  char logname[200];
  char *logdir = getenv("AUV_LOG_DIR");

  // Find a unique file name  
  for (int fi = 0; ; fi++)
  {
    sprintf(logname, "%s/AmcAgent.log.%d", logdir, fi);
    if (NULL == (_log = fopen(logname, "r")))
       break;       // No such file
    else
      fclose(_log); // Close the existing file
  }
  
  if (NULL == (_log = fopen(logname, "w")))
  {
    D_PRINTF("Could not open %s, using stdout\n", logname);
  }
  else
  {
    long now;
    time(&now);
    log("\n######################### %s\n", ctime(&now));
  }

  AmcMessage::Message msg;
  _msgs = new AmcMessage(MessageQueue::ReadWrite, AmcMessageQueueName);
  if (_msgs->msgsPending() > 0)
    while ((_msgs->read(&msg)) > 0)
    {
     	log("AmcAgent::init() - flushing leftover message: %d/%d",
          msg._msg, msg._behaviorId);
    }
    
  _msgs->flush();
  
}


AmcAgent::~AmcAgent()
{
  if (_socket) close(_socket);
  if (_client) close(_client);
  if (_msgs) delete(_msgs);
  if (_layeredControlIF) delete _layeredControlIF;
}

///////////////////////////////////////////////////////////////////////////////
// Initialize agent - read config attrs and set-up a socket
//
int AmcAgent::init()
{
  char configFile[100];
  strcpy(configFile, System::configurationFile(_config));

  createCfgAttributes();
  loadConfigFile(configFile);
  System::copyToLogDir(configFile);
  reportCfgAttributes();

  // Create socket
  //
  setupSocket();
  if (_socket < 0)
  {
    log("AmcAgent: Unable to initialize socket");
    return -1;
  }

  return 0;
}

///////////////////////////////////////////////////////////////////////////////
// Open interface to Layered Control
//
int AmcAgent::openLC()
{
  // Create interfaces servers
  //
  try {
    _layeredControlIF = new LayeredControlIF("layeredControl");
    log("AmcAgent::openLC() - created IF with LayeredControl");
  }
  catch (Exception e) {
    log("AmcAgent::openLC() - failed creating LayeredControlIF: %s", e.msg);
    return -1;
  }
  return 0;
}

static long _cyc = 1;
///////////////////////////////////////////////////////////////////
// Check for messages from LayeredControl that must be forwarded
// to AMC
//
void AmcAgent::handleLCMessages()
{
  AmcMessage::Message msg;
  
  while ((_msgs->read(&msg)) > 0)
  {
    switch (msg._msg) {
      case AmcMessage::BehaviorStarted:
        if (msg._behaviorId >= 0)
        {
          ackAmc(msg._behaviorId, "STARTED");
          System::milliSleep(40);  // small delay stops AMC from dropping msgs
        }
        break;
        
      case AmcMessage::BehaviorFinished:
        if (msg._behaviorId > 0)
        {
          ackAmc(msg._behaviorId, "FINISHED");
          System::milliSleep(40);  // small delay stops AMC from dropping msgs
        }

        // This is the event that signals to the AMC that
        // the mission has started
        if(0 == msg._behaviorId && !_missionStarted)
        {
         	log("AmcAgent::handleLCMessages() - AMC mission started");
          time(&_msgTime);
          _missionStarted = True;

          ackAmc(msg._behaviorId, "FINISHED");
          System::milliSleep(40);  // small delay stops AMC from dropping msgs
        }
        break;
        
      default:
      	log("AmcAgent::handleLCMessages() - Unknown command: %d", msg._msg);
        break;
    }
  }
}


///////////////////////////////////////////////////////////////////////////////
// Set-up a socket for conversation with AMC
//
int AmcAgent::setupSocket()
{
  int  sockfd;

  // Initialize our socket to an unuseable value
  //
  _socket = -1;
  
  // Attempt to setup a UDP client socket on the AMC port
  // We will send state packets to the AMC server
  //
  D_PRINTF("AmcAgent: Socket params - Port:%d\n", _port);
  
  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);

  // Get a socket resource
  //
  if ( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
    D_PRINTF("AmcAgent: socket init failed (socket() call = %d)\n", sockfd);
    return sockfd;
  }

  // Fix things so that we can restart amcAgent right away
  // without having to wait until the system clears the socket
  //
  int opt = 1;
  if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof (opt)) < 0)
  {
    D_PRINTF("AmcAgent: setsockopt failed: %d\n", errno);
    return -1;
  }


  if (bind(sockfd, (struct sockaddr*)&_servaddr, sizeof(_servaddr)) < 0)
  {
    D_PRINTF("AmcAgent: binding client socket failed: %d\n", errno);
    return -1;
  }

  // Socket is working
  //
  log("AmcAgent: Socket created on fd %d", sockfd);
  _socket = sockfd;  

  return 0;
}

///////////////////////////////////////////////////////////////////////////////
// Listen for client connection
//
int AmcAgent::acceptClient()
{
  D_PRINTF("AmcAgent: Waiting for client to connect...\n");
  listen(_socket, 1);

  // Accept the connection
  //  
  int clilen = sizeof(_cliaddr);
  if ((_client = accept(_socket, (struct sockaddr*)&_cliaddr, &clilen)) < 0)
  {
    log("AmcAgent: accept() error for client. Errno = %d", errno);
    return - 1;
  }
  log("AmcAgent: Connected to client on %s", inet_ntoa(_cliaddr.sin_addr));

  time(&_msgTime);  // Record time when message was received
  return 0;
}

// ########### Configuration file attributes section ###########

///////////////////////////////////////////////////////////////////////////////
// Create attributes in our attributes member
//
void AmcAgent::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 ) );

}

///////////////////////////////////////////////////////////////////////////////
// Load the config attributes and do some sanity checking
//
void AmcAgent::loadConfigFile(const char* configFile)
{
  AttributeParser::parse(configFile, &_attributes); 

  if (_port < 2000)
  {
    log("AmcAgent: Port %d out of bounds, defaulting to %d", _port, DEFAULT_PORT);
    _port = DEFAULT_PORT;
  }
}

///////////////////////////////////////////////////////////////////////////////
// Record the attributes in Syslog
//
void AmcAgent::reportCfgAttributes()
{
  log("AmcAgent -- configuration:\n\tPort = %d ms", _port);
}

// ########### AMC Request read and handle section ###########

#define MSGBUFLEN 1024

///////////////////////////////////////////////////////////////////////////////
// Accept a client connection then wait for and execute commands
//
void AmcAgent::run()
{
  log("AmcAgent - running");
  if (_socket < 0) return;
  
  // Wait for client to connect
  //
  if (0 != acceptClient())
    return;
    
  // Client request loop
  // Read request message and handle it. Stop when client requests an "exit"
  //
  Boolean clientDied = False;
  Boolean done = False;
  int n;
  int bufsize = 2048;
  char *msgbuf = (char*)malloc(bufsize);
  while (!done)
  {
    handleLCMessages();
    
    // Make sure client socket is set for non-blocking I/O
    //
    int flags = fcntl(_client, F_GETFL);
    fcntl(_client, F_SETFL, flags | (O_NONBLOCK));
    
    // Set-up for select()ing and select() on the client socket
    //
    FD_ZERO(&_fdset);
    FD_SET(_client, &_fdset);
    struct timeval to;
    to.tv_sec = 0;
    to.tv_usec = 50000L;

//    D_PRINTF("AmcAgent: Waiting for client to talk...\n");
    int retval;
    if (retval = (select(_client+1, &_fdset, (fd_set*)0, (fd_set*)0, &to)) < 0)
    {
      // Error condition
      log("AmcAgent: select() error. Errno = %d", errno);
      done = True;
      continue; 
   }

    // If there is a message, handle it
    //
    if (n = (recv(_client, msgbuf, bufsize, MSG_PEEK)) > 0)
    {
      // Read message making sure the buffer is large enough to hold it all.
      //
      if (n >= bufsize)
      {
        // Add more to the buffer so it is large enough
        //
        bufsize = n;
        msgbuf = (char*)realloc(msgbuf, bufsize);
      }
      D_PRINTF("AmcAgent: There are %d bytes waiting for buffer of size %d\n",
               n, bufsize);

      // Now read the message
      //
      n = recv(_client, msgbuf, bufsize, 0);
      if (n <= 0)
        log("AmcAgent: Recv problem: Errno = %d", errno);

      else if (n > 0)
      {
        time(&_msgTime);  // Record time when message was received
        _sdelta = 0;

        // Exit when requested to
        //
        if (0 == strcasecmp(msgbuf, "exit"))
        {
          log("AmcAgent: Exit requested");
          close(_client); _client = 0;
          close(_socket); _socket = 0;
          if (_layeredControlIF) _layeredControlIF->abortMission();
          done = True;
          continue; 
      }

       log("AmcAgent::run() - received msg - %s", msgbuf);
 
// Note when we received latest ping pulse
        //
        if (0 == strcasecmp(msgbuf, "ping"))
        {/*
          if (n > (strlen("ping")+1))
          {
            msgbuf = msgbuf + strlen("ping")+1;
            log("AmcAgent::run() - Message stacked on top of ping - %s", msgbuf);
          }
          else*/
            continue;  // Just a ping, don't send msg to handler
        }

        // Parse message and act upon it
        //
        handleAmcRequest(msgbuf);
      }
    }
    
    // No message (select timed-out)
    // Should we assume AMC has hung-up on us?
    //
    else
    {
      long now; time(&now);
      long delta = now - _msgTime;
      if (delta != _sdelta && (delta > (_hupTime/2)))
      {
        log("AmcAgent::run() - no ping received for %d of up to %d seconds",
        delta, _hupTime);
        _sdelta = delta;
      }
      
      if (_missionStarted && (delta > _hupTime))
      {
        log("AmcAgent::run() - Hang-up timeout %d sec exceeded - aborting!",
            _hupTime);
        if (_layeredControlIF) _layeredControlIF->abortMission();
        done=True;
        continue; 
    }
      
      if (errno == ENOMSG)
      {
        // This situation usually means the client AMC exited early
        // Sleep extra rather than run wild
        //
        if (!clientDied)
        {
          log("AmcAgent::run() - client died?");
          clientDied = True;
        }
        System::milliSleep(50);
      }
    }
  }
  // Done with processing
  if (msgbuf) free (msgbuf);
}

///////////////////////////////////////////////////////////////////////////////
// Parse command string and call appropriate handler
//
void AmcAgent::handleAmcRequest(const char* msg)
{
  // Ensure the format of the message
  //
  if (!strstr(msg, CMD_DELIMITER))
  {
    nackAmc(0, "BadCmdFormat");
    log("AmcAgent: Bad command format: %s", msg);
    return;
  }
  
  // Get the command and command body
  //
  char *body = strstr(msg, CMD_DELIMITER);
  *body = '\0';
  body++;

  // Handle the command
  //
  int ret;
  if (!strcasecmp(msg, "append"))
  {
    ret = appendBehaviors(body);
  }
  else if (!strcasecmp(msg, "delete"))
  {
    log("AmcAgent: handling deleteBehavior");
    ret = deleteBehavior(body);
  }
  else if (!strcasecmp(msg, "insert"))
  {
    log("AmcAgent: handling insertBehavior");
    ret = insertBehaviors(body);
  }
  else if (!strcasecmp(msg, "init"))
  {
    ret = initMission(body);
    openLC();
  }
  else
  {
    log("AmcAgent::handleAmcRequest - UnknownCommand %s", msg);
    return;
  }

  return;
}

///////////////////////////////////////////////////////////////////////////////
// write behaviors to disk and verify them with checkplan
//
int AmcAgent::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 (bf)
  {
    fwrite(behaviors, strlen(behaviors), 1, bf);
    fwrite("\n", 1, 1, bf);
    fflush(bf);
    fclose(bf);
  }
  else
  {
    log("AmcAgent:: - could not open %s", file);
    nackAmc(id, "File open error");
    return -1;
  }
 
  return 0;
  //Skip checkplan for now
   
  // Run checkplan on them to verify
  //
  char cpcom[300];
  sprintf(cpcom, "checkplan %s", file);
  if (0 != system(cpcom))
  {
    log("AmcAgent:: - %s failed", cpcom);
    nackAmc(id, "Checkplan error");
    return -1;
  }
  log("AmcAgent:: - %s succeeded", cpcom);
  return 0;
}

///////////////////////////////////////////////////////////////////////////////
// Insert behaviors into the stack after the Idle behavior (id = 0)
// Return 0 if successful
//
int AmcAgent::insertBehaviors(const char* behaviors)
{
  D_PRINTF("AmcAgent:insertBehaviors:\n%s", 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;
  FILE *bf;
  
  // Create mission file to parse
  //
  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);
    nackAmc(id, "insert");
    return -1;
  }
}

///////////////////////////////////////////////////////////////////////////////
// Append (push) behaviors on the stack
// Return 0 if successful
//
int AmcAgent::appendBehaviors(const char* behaviors)
{
  log("AmcAgent:appendBehaviors:\n%s", 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;
  FILE *bf;
  
  // Create mission file to parse
  //
  sprintf(bfilename, "%s/append%d.cfg", dir, _appendNum++);
  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);
    nackAmc(id, "append");
    return -1;
  }
    
}

///////////////////////////////////////////////////////////////////////////////
// Initialize the mission
// Return 0 if successful
//
int AmcAgent::initMission(const char* mission)
{
  log("AmcAgent:initMission:\n%s", mission);

  // Write mission to a file and invoke Supervisor
  // Place the file in the log directory
  //
  char* dir = getenv("AUV_PLAN_DIR");
  char mfile[200];
  FILE *bf;
  
  // Create mission file to parse
  //
  sprintf(mfile, "%s/AmcPlan.cfg", dir);
  if (0 != writeAndVerify(mission, mfile))
    return -1;

  // Run supervisor and point it at the mission file
  //
  if (fork() == 0)
  {
    log("AmcAgent::initMission() - starting supervisor");
    // Child process
    execlp("supervisor", "supervisor", "-plan",
           "AmcPlan.cfg", "-dyno", _sim, (char*)0);
    D_PRINTF("AmcAgent - Supervisor child process exiting\n");
    exit(1);
  }
  
  return 0;
}

///////////////////////////////////////////////////////////////////////////////
// Remove a behavior from the stack
// Return 0 if successful
//
int AmcAgent::deleteBehavior(const char* id)
{
  long n_id = atoi(id);
  log("AmcAgent:deleteBehavior:\n%d", n_id);
  
  if (_layeredControlIF &&
      LayeredControlIF::Ok == _layeredControlIF->deleteBehavior(n_id))
    return 0;
  else
  {
    nackAmc(atol(id), "delete");
    return -1;
  }
}

///////////////////////////////////////////////////////////////////////////////
void AmcAgent::ackAmc(long id, const char* msg)
{
  char *ack = new char[strlen(msg)+20];
  sprintf(ack, "%s:%d", msg, id);
  
  log("AmcAgent: ack - %s", ack);
  int nb = send(_client, ack, strlen(ack)+1, 0);
  if (nb != (strlen(ack)+1)) log("AmcAgent: ack - sent only %d bytes of %d");
  delete ack;
}

///////////////////////////////////////////////////////////////////////////////
void AmcAgent::nackAmc(long id, const char* msg)
{
  char *nack = new char[strlen(msg)+strlen(NACK_CMD)+20];
  sprintf(nack, "%s:%d:%s", NACK_CMD, id, msg);
  
  log("AmcAgent: Nack - %s", nack);
  send(_client, nack, strlen(nack)+1, 0);
  delete nack;
}

// Log function
//
void AmcAgent::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);
}