/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : StartSurvey.cc                                                */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 08/23/2008                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "StartSurvey.h"
#include "Syslog.h"


StartSurvey::StartSurvey()
  : Behavior(StartSurveyBehaviorName, NonSequential)
{
  Boolean debug = False;

  if (!_modem) {
	try {
		_modem = new AcousticModemIF("LayeredControl",
					     "BenthosModemServer", 20);
        }
	catch (...) {
		Syslog::write("StartSurvey:: Failed to initialize connection "
			      "with acoustic modem\n");
		_modem = NULL;
		exit(-1);
        }
   }
}


StartSurvey::~StartSurvey()
{
	if (_modem) delete _modem;
}


Boolean StartSurvey::shouldBehaviorStart( void ) {
     return bothSequence();
}

void StartSurvey::execute()
{
double now = _missionClock->seconds();
Boolean newdata;
AcousticModemIF::SMAC_CMD_TYPE cmdRec;

     if (_modem) {
	_modem->getSMACCmd(&cmdRec, &newdata);
     }
 
     if ( ((now - startTime()) > _timeout) &&
	  cmdRec != AcousticModemIF::START_SURVEY) {
	Syslog::write("StartSurvey - Timed out without getting a message");
        setState(Finished);
        abortMission();
	return;
     }

     setHorizontal(DynamicControlIF::Heading, 0.0);
     setSpeed(DynamicControlIF::Speed, 0.0);
     setVertical(DynamicControlIF::Pitch, 0.0);
}


Boolean StartSurvey::validInput()
{
  //set up the timeout value to be the duration; adjust duration 10 seconds
  //upward so we don't get short-shrifted
  _timeout = duration();
  _duration += 10;
  return True;
}