/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DynoLayeredControl.cc                                         */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/17/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <errno.h>
#include <unistd.h>
#include "malloc.h"
#include "DynoLayeredControl.h"
#include "MissionPlan.h"
#include "Behavior.h"
#include "Syslog.h"
#include "EventLogIF.h"
#include "VcsMessage.h"

#define DEBUG True

DynoLayeredControl::DynoLayeredControl(const char *planName,
                                       const char *abortPlan, int period)
  : LayeredControl(planName, abortPlan, period),
    _cmdInput(NULL), _cmdOutput(NULL)
{
  Boolean debug = DEBUG;
  dprintf("DynoLayeredControl ctor\n");
  _cmdInput = new DynoCommand(MessageQueue::ReadWrite, DynoInputQueueName);
  _cmdOutput = new DynoCommand(MessageQueue::ReadWrite, DynoOutputQueueName);
  _cmdInput->flush();
  _cmdOutput->flush();
  
  _vcsMessages = new VcsMessage(MessageQueue::ReadWrite, VcsMessageQueueName);
}


DynoLayeredControl::~DynoLayeredControl()
{
  if (_cmdInput)  delete _cmdInput;
  if (_cmdOutput) delete _cmdOutput;
  if (_vcsMessages) delete _vcsMessages;
}

void DynoLayeredControl::initialize(const char *plan, const char *abortPlan)
{
  Boolean debug = DEBUG;
  dprintf("DynoLayeredControl initialize\n");
  LayeredControl::initialize(plan, abortPlan);
}

void DynoLayeredControl::callback()
{
  Boolean debug = DEBUG;
  // Do DynoLayeredControl stuff
  // Execute any commands that are in the queue
  //
  DynoCommand::Command cmdIn;
  while ((_cmdInput->read(&cmdIn)) > 0)
  {
    switch (cmdIn._cmd) {
      case DynoCommand::setNewBehaviorTimeoutLimit:
	setVcsNewBehaviorTimeoutLimit((unsigned)cmdIn._arg1);
        break;
        
      case DynoCommand::appendBehaviors:
	appendBehaviors((const char*)cmdIn._file);
        break;
        
      case DynoCommand::insertBehaviors:
	insertBehaviors((const char*)cmdIn._file, cmdIn._behaviorId);
        break;
        
      case DynoCommand::deleteBehavior:
	deleteBehavior(cmdIn._behaviorId);
        break;
        
      default:
	Syslog::write("DynoLayeredControl::callback() - Unknown command: "
		      "%d", cmdIn._cmd);
        break;
    }
  }
  
  // Let LayeredControl parent class do its' thing
  LayeredControl::callback();
}

void DynoLayeredControl::appendBehaviors(const char* bfile)
{
  if (LayeredControlIF::Aborting == _missionState)
  {
    Syslog::write("DynoLayeredControl::append - what's the point, already in abort phase?");
    respond(DynoCommand::appendBehaviorsNack, LayeredControlIF::OperationFailed);
    return;
  }
  
  // First make sure the file is there
  //
  struct stat sb;
  if (stat(bfile, &sb) == ENOENT)
  {
    Syslog::write("DynoLayeredControl::append - file not found");
    respond(DynoCommand::appendBehaviorsNack, LayeredControlIF::FileNotFound);
    return;
  }
  else
  {
//    System::copyToLogDir(bfile);
  }

  // Try to load file and verify behaviors
  //
  BehaviorStack subStack;
  MissionPlan mp;
  Boolean loaded = False;
  for (int nt = 0; nt < 3; nt++)
  {
    if (mp.load(bfile, &subStack) != -1)
    {
      loaded = True;
      break;
    }
  }
  
  if (!loaded)
  {
    Syslog::write("DynoLayeredControl::append - file would not load");
    respond(DynoCommand::appendBehaviorsNack, LayeredControlIF::BadBehaviorFormat);
    return;
  }

  // Append the sub-stack onto the main stack
  //
#if 0
  Syslog::write("DynoLayeredControl::append - appending %d behaviors", subStack.size());
#endif
  Behavior* b;
  for (int si = 0; si < subStack.size(); si++)
  {
    subStack.get(si, &b);
    _normalBehaviorStack.add(&b);
  }
#if 0
  Syslog::write("DynoLayeredControl::append - stack now contains %d behaviors",
                 _normalBehaviorStack.size());
#endif
  Syslog::write("DynoLayeredControl::append - top priority behavior is %s",
                 b->name());
  respond(DynoCommand::appendBehaviorsAck, LayeredControlIF::Ok);
}

void DynoLayeredControl::insertBehaviors(const char* bfile, long after_here)
{
  if (LayeredControlIF::Aborting == _missionState)
  {
    Syslog::write("DynoLayeredControl::insert - what's the point, already in abort phase?");
    respond(DynoCommand::appendBehaviorsNack, LayeredControlIF::OperationFailed);
    return;
  }
  
  // First find the behavior with the id == after_here
  //
  Behavior* zerob;
  Boolean bfound = False;
  int ni;
  for (ni = 0; !bfound && ni < _normalBehaviorStack.size(); ni++)
  {
    _normalBehaviorStack.get(ni, &zerob);
    if (zerob->id() == after_here)
    {
      bfound = True;
      break;
    }
  }
  
  if (!bfound)
  {
    Syslog::write("DynoLayeredControl::insert - no such behavior id = %d", after_here);
    respond(DynoCommand::insertBehaviorsNack, LayeredControlIF::BehaviorNotFound);
    return;
  }

  // Then make sure the file is there
  //
  struct stat sb;
  if (stat(bfile, &sb) == ENOENT)
  {
    Syslog::write("DynoLayeredControl::insert - file not found");
    respond(DynoCommand::insertBehaviorsNack, LayeredControlIF::FileNotFound);
    return;
  }
  else
  {
//    System::copyToLogDir(bfile);
  }

  // Try to load file and verify behaviors
  //
  BehaviorStack subStack;
  MissionPlan mp;
  Boolean loaded = False;
  for (int nt = 0; nt < 3; nt++)
  {
    if (mp.load(bfile, &subStack) != -1)
    {
      loaded = True;
      break;
    }
  }
  
  if (!loaded)
  {
    Syslog::write("DynoLayeredControl::insert - file would not load");
    respond(DynoCommand::insertBehaviorsNack, LayeredControlIF::BadBehaviorFormat);
    return;
  }

  // Insert the sub-stack onto the main stack
  //
#if 0
  Syslog::write("DynoLayeredControl::insert - inserting %d behaviors", subStack.size());
#endif
  Behavior* b;
  for (int si = 0; si < subStack.size(); si++)
  {
    subStack.get(si, &b);
#if 0
    Syslog::write("DynoLayeredControl::insert - insertion %s after pos %d",
                   b->name(), ni);
#endif
    int j;
    if (0 != (j = _normalBehaviorStack.insert(ni, &b)))
    {
      Syslog::write("DynoLayeredControl::insert - insertion failed %d", j);
      respond(DynoCommand::insertBehaviorsNack, LayeredControlIF::BadBehaviorFormat);
      return;
    }
  }
  Syslog::write("DynoLayeredControl::insert - stack now contains %d behaviors",
                 _normalBehaviorStack.size());

  // Let server know we're done
  //
  respond(DynoCommand::insertBehaviorsAck, LayeredControlIF::Ok);
}

void DynoLayeredControl::deleteBehavior(long id)
{
  Boolean debug = DEBUG;
  
  // Find the behavior in the behavior stack
  //
#if 0
  Syslog::write("DynoLayeredControl::delete - deleteing behavior w/id=%d", id);
#endif
  Behavior* b;
  Boolean bfound = False;
  int si;
  for (si = 0; !bfound && si < _normalBehaviorStack.size(); si++)
  {
    _normalBehaviorStack.get(si, &b);
    if (b->id() == id)
    {
      bfound = True;
#if 0
      dprintf("Found behavior %s with id %d\n", b->name(), id);
#endif
      break;  // conserve value of si for use later
    }
  }
  
  // Couldn't find the behavior? Report and return
  //
  if (!bfound)
  {
    Syslog::write("DynoLayeredControl::delete - behavior not found");
    respond(DynoCommand::deleteBehaviorNack,LayeredControlIF::BehaviorNotFound);
    return;
  }
    
  // If the behavior is currently active, hasten its demise.
  // If the behavior is ready to execute but hasn't started,
  // take it out of the stack. Otherwise, just let it be.
  //
  int rem = 0;
  if (b->state() == Behavior::Active)
  {
    dprintf("Behavior %s is Active, abandon it\n", b->name());
    b->abandon(); 
  }
  else if (b->state() == Behavior::Suspended)
  {
    dprintf("Behavior %s is not Active, remove it from the stack\n", b->name());
    Behavior *temp;
    rem = _normalBehaviorStack.remove(si, &temp);
//    delete (temp);
  }
  
  // Let caller know how things went
  if (rem != 0)
  {
    Syslog::write("DynoLayeredControl::delete - behavior not removed");
    respond(DynoCommand::deleteBehaviorNack,LayeredControlIF::BehaviorNotFound);
  }
  else
  {
#if 0
    Syslog::write("DynoLayeredControl::delete - behavior removed");
#endif
    respond(DynoCommand::deleteBehaviorAck, LayeredControlIF::Ok);
  }
  return;
}

void DynoLayeredControl::respond(DynoCommand::DynoCommands c,
                                 LayeredControlIF::Status stat)
{
  DynoCommand::Command resp(c, stat);
  _cmdOutput->write(&resp);
}