/****************************************************************************/
/* 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 ) );
//
// Moved this into Layered Control where the other interfaces reside.  This
// avoids needlessly allocating memory for the interface in each instance of
// the behavior, and also allows checkplan to work.
//
// 3 May 2006 rsm.  Moved it back, but made it a static member variable of
// class Behavior so that the memory need only be allocated once, and
// subsequent behaviors can use the same server.  See the comment in
// LayeredControl.cc
//
#if 0

  if( _vehicleConfig->useIns() == 1. ) 
  {
     try
     {
	if( !_ins ) _ins = new InsIF("kearfott", INS_AHRS_TIMEOUT);
     }
     catch(...)
     {
	Syslog::write("GetGPS::GetGPS() -- Failed to initialize connection "
		      "to InsIF");
	_ins = NULL;
     }
  }

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

#endif

  _hitsCounted = 0;
  _hitsCountedIns = 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 = True;

  // 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);


  InsIF::InertialState inertialState;
  TimeIF::TimeSpec insSampleTime;
  long status;
  unsigned short cycles;
  unsigned char mode;
  unsigned char monitor;
  //
  // Read and parse the Ins status, if the Ins is present:
  //
  if( _vehicleConfig->useIns() == 1. )
  {
     _ins->getInertialState( &inertialState, &insSampleTime );
     status = inertialState.status;

     cycles    = ( 0x0000FFFF &   status         );
     mode      = ( 0x000000FF & ( status >> 16 ) );
     monitor   = ( 0x000000FF & ( status >> 24 ) );
  }


  if( fix.quality != GpsIF::Invalid ) 
  {
    dprintf(" GetGPS: fix.quality = %d\n", fix.quality);
    //Syslog::write("GetGPS - time is %d\n", fix.sampleTime.seconds);
    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( _vehicleConfig->useIns() == 1. )
      {
	 Syslog::write("GetGPS::execute():\n"
		       "   Depth loop open > 1 minute: %d\n"
		       "   Gps data processed:         %d\n"
		       "   Gps data rejected:          %d\n"
		       "   Dvl data processed:         %d\n"
		       "   Dvl data rejected:          %d\n"
		       "   ZUPT processed              %d\n"
		       "   Valid Dvl height            %d\n",
		       0x01 & (monitor >> 7),
		       0x01 & (monitor >> 6),
		       0x01 & (monitor >> 5),
		       0x01 & (monitor >> 4),
		       0x01 & (monitor >> 3),
		       0x01 & (monitor >> 2), 
		       0x01 & ( monitor    )  );
	 //
	 // Increment hitsCountedIns if the Gps Processed bit is set, which
	 // is bit 7 (2^6 or 0x40).
	 if( 0x40 & monitor ) 
	 {
	    _hitsCountedIns++;
	    Syslog::write(
	       "GetGPS::execute() -- The Keafott accepted the Gps hit: "
	       "%d of %d needed",
	       _hitsCountedIns, _hitsNeeded );
	 }

	 if( _hitsCountedIns >= _hitsNeeded ) 
	 {
	    setState(Finished);
	    return;
	 }
      }
      else
      {
	 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;
}