/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DynoLayeredControlServer.cc                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/23/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>
#include "DynoLayeredControlServer.h"
#include "System.h"
#include "Syslog.h"

DynoLayeredControlServer::DynoLayeredControlServer( int nArgc, char **nArgv )
  : LayeredControlServer(nArgc, nArgv)
{
  Syslog::write("DynoLayeredControlServer ctor");
  _input  = new DynoCommand(MessageQueue::ReadWrite, DynoOutputQueueName);
  _output = new DynoCommand(MessageQueue::ReadWrite, DynoInputQueueName);
}


DynoLayeredControlServer::~DynoLayeredControlServer()
{
  if (_input) delete _input;
  if (_output) delete _output;
}

void DynoLayeredControlServer::setNewBehaviorTimeoutLimit(long limit_in_seconds)
{
  DynoCommand::Command cmd;
  cmd._cmd = DynoCommand::setNewBehaviorTimeoutLimit;
  cmd._arg1 = (short)limit_in_seconds;
  _output->write(&cmd);

}

LayeredControlIF::Status DynoLayeredControlServer::appendBehaviors(
                         LayeredControlIF::BehaviorFilename file)
{
  Syslog::write("DynoLayeredControlServer appendBehaves");

  // Send a append message to the driver
  //  
  _input->flush();  // discard pending messages
  DynoCommand::Command cmd(DynoCommand::appendBehaviors, file);
  _output->write(&cmd);

  return(LayeredControlIF::Ok);


  // Get a response from the driver
  //
  LayeredControlIF::Status ret = getDriverResponse();
  Syslog::write("DynoLayeredControlServer::appendBehaviors: return status = %d",
                ret);
  return ret;
}

LayeredControlIF::Status DynoLayeredControlServer::insertBehaviors(
                         LayeredControlIF::BehaviorFilename file,
                         long behavior_id)
{
  Syslog::write("DynoLayeredControlServer insertBehaves");
  
  // Send a insert message to the driver
  //  
  _input->flush();  // discard pending messages
  DynoCommand::Command cmd(DynoCommand::insertBehaviors, file, behavior_id);
  _output->write(&cmd);

  return(LayeredControlIF::Ok);


  // Get a response from the driver
  //
  LayeredControlIF::Status ret = getDriverResponse();
  Syslog::write("DynoLayeredControlServer::insertBehaviors: return status = %d",
                ret);
  return ret;
}

LayeredControlIF::Status DynoLayeredControlServer::deleteBehavior(long behavior_id)
{
  Syslog::write("DynoLayeredControlServer rdeleteBehavior(%d)", behavior_id);
  
  // Send a delete message to the driver
  //  
  _input->flush();  // discard pending messages
  DynoCommand::Command cmd(DynoCommand::deleteBehavior, behavior_id);
  _output->write(&cmd);

  return(LayeredControlIF::Ok);


  // Get a response from the driver
  //
  LayeredControlIF::Status ret = getDriverResponse();
  Syslog::write("DynoLayeredControlServer::deleteBehavior: return status = %d",
                ret);
  return ret;
}

int DynoLayeredControlServer::spawnAuxTasks()
{
  Syslog::write("DynoLayeredControlServer spawnAuxTasks");
  
  // Should we start the AmcAgent here?
  
  // Start the other LC tasks
  //
  LayeredControlServer::spawnAuxTasks();
  return 0;
}

LayeredControlIF::Status DynoLayeredControlServer::getDriverResponse(unsigned int milliWait)
{
  DynoCommand::Command resp;
  LayeredControlIF::Status ret;
  // Wait for response from driver
  //
  int sleepTime, sleepIncr = 10;
  for (sleepTime = 0; sleepTime < milliWait &&
                      _input->msgsPending() <= 0; sleepTime += sleepIncr)
  {
    System::milliSleep(sleepIncr);
  }

  // If message is there, get it, otherwise use default cmd
  //
  if (_input->msgsPending() > 0)
  {
    _input->read(&resp);
    Syslog::write("DynoLayeredControlServer::getDriverResponse() waited ~%dms",
                sleepTime);
  }
  else
  {
    Syslog::write("DynoLayeredControlServer:: getDriverResponse() timed-out");
    return LayeredControlIF::Ok;
  }

  // Is it good?
  //  
  if (resp._cmd == DynoCommand::appendBehaviorsAck ||
      resp._cmd == DynoCommand::insertBehaviorsAck ||
      resp._cmd == DynoCommand::deleteBehaviorAck)
    ret = LayeredControlIF::Ok;
  // Is it bad?
  //  
  else if (resp._cmd == DynoCommand::appendBehaviorsNack ||
           resp._cmd == DynoCommand::insertBehaviorsNack ||
           resp._cmd == DynoCommand::deleteBehaviorNack)
    ret = resp._arg2;
  // Is it unknown?
  //  
  else if  (resp._cmd == DynoCommand::None)
    ret = LayeredControlIF::OperationFailed;
  else
  {
    Syslog::write("DynoLayeredControlServer:: Unexpected response %d",
                  resp._cmd);
    ret = LayeredControlIF::OperationFailed;
  }
  return ret;
}