/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : GetGPS.cc                                                     */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "GetGPS.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "IntegerAttribute.h"
#include "BooleanAttribute.h"
#include "Syslog.h"

GetGPS::GetGPS()
  : Behavior(GetGPSBehaviorName, NonSequential)
{

  attributes.add(new BooleanAttribute("abortOnTimeout",
				      "Should mission abort if insufficient "
                                      "# hits at end of behavior?",
				      &_abortOnTimeout,
				      False ) );
  attributes.add(new IntegerAttribute("minHits",
				      "Minimum number of good GPS hits "
				      "to collect",
				      &_hitsNeeded,
				      1 ) );

  try {
    _gps = new GpsIF("gps");
  } 
  catch(...)
  {
    Syslog::write("GetGPS::GetGPS() -- Failed to initialize connection to GPS IF");
  }

  _hitsCounted = 0;
  _timeout = 0.0;
  _prevTime = 0;
}


GetGPS::~GetGPS()
{
}

Boolean GetGPS::shouldBehaviorStart()
{
  return bothSequence();
}


void GetGPS::execute()
{
  double now = _missionClock->seconds();
  short qual;
  long newTime;
  Boolean debug = False;

  // Check for launch timeout, but only if you're going to abort
  if( _abortOnTimeout && 
      ( now - startTime() > _timeout ) ) {
    Syslog::write("GetGPS::execute() -- Timed out without getting required number of hits.  Aborting...");
       setState(Finished);
       abortMission();
       return;
  }


  GpsIF::Fix fix;

  _gps->getFix(&fix);

  if( fix.quality != GpsIF::Invalid ) {
    dprintf(" GetGPS: fix.quality = %d\n", fix.quality);
    if( fix.sampleTime.seconds > _prevTime ) {
      _prevTime = fix.sampleTime.seconds;
      _hitsCounted++;

      Syslog::write("GetGPS::execute() -- Got a new GPS hit: %d of %d needed",
		    _hitsCounted, _hitsNeeded );

      if( _hitsCounted >= _hitsNeeded ) {
	setState(Finished);
	return;
      }
    }
  }


  // Set a good neutral floating position
  setHorizontal(DynamicControlIF::Rudder, 0.);
  setVertical(DynamicControlIF::Depth, 0 );
  setSpeed(DynamicControlIF::Speed, 0 );
}


Boolean GetGPS::validInput()
{
  Boolean valid = True;

  // Take over the timeout function if abortOnTimeout is set
  if( _abortOnTimeout ) {
       _timeout = duration();
       _duration += 10;
  }

  return valid;
}