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

#define NotSpecified -100000.0
#define AngleNotSpecified (-1000. / Math::RadsPerDeg)

DownloadData::DownloadData()
  : Behavior(DownloadDataBehaviorName, NonSequential)
{
  Boolean debug = False;

  _buoyLauncher = new BuoyLauncherIF("buoyLauncher");

  attributes.add(new AngleAttribute("heading", "Commanded heading",
				    &_heading));

  attributes.add(new FloatAttribute("speed", "Commanded speed", &_speed));

  attributes.add(new VerticalModeAttribute("verticalMode", 
					   "Either \"depth\" or \"pitch\"",
					   &_verticalMode,
					   DynamicControlIF::VmInitial));

  _depthAttribute = new FloatAttribute("depth",
				       "Commanded depth if verticalMode "
				       "is \"depth\"",
				       &_depth, 
				       NotSpecified);

  attributes.add(_depthAttribute);


  attributes.add(new IntegerAttribute("statusCalls", 
				      "Turn status calls on?",
				      &_statusCalls, NullLegDuration));

  attributes.add(new IntegerAttribute("archiveData", 
				      "Download Archive Data?",
				      &_archiveData, NullLegDuration));

  attributes.add(new StringAttribute("dataFilename", 
				      "Data file to download",
				      &_dataFilename, "USE_DEFAULT"));

  _downloadDelayAttr = new FloatAttribute("downloadDelay",
				       "Delay until download "
				       "is \"delay\"",
				       &_downloadDelay, 
				       NotSpecified);

  attributes.add(_downloadDelayAttr);


  _statusDelayAttr = new FloatAttribute("statusDelay",
				       "Delay until rotate "
				       "is \"delay\"",
				       &_statusDelay, 
				       NotSpecified);

  attributes.add(_statusDelayAttr);


  _pitchAttribute = new AngleAttribute("pitch", 
				       "Commanded pitch if verticalMode "
				       "is \"pitch\"",
				       &_pitch, 
				       AngleNotSpecified);

  attributes.add(_pitchAttribute);

  dprintf("DownloadData::DownloadData() - pitch=%.3f", _pitch);

  attributes.add(new IntegerAttribute("legDuration", 
				      "Duration of each leg",
				      &_legDuration, NullLegDuration));

  attributes.add(new IntegerAttribute("maxLegs", 
				      "Maximum number of legs",
				      &_maxLegs, 1));

  attributes.add(new AngleAttribute("headingInc", 
				    "Increment heading by this amount "
				    "after each leg",
				    &_headingIncrement, 
				    0.));

  dprintf("DownloadData::DownloadData() - #2: pitch=%.3f", _pitch);

  _legsCompleted = 0;
  _legStartTime = NotSpecified;
  //
  // Initialize download stuff:
  //
  _downloaded       = False;
  _initiateDownload = False;
  _statusSet = False;

}


DownloadData::~DownloadData()
{
}


Boolean DownloadData::shouldBehaviorStart( void ) {

     return horizontalSequence();
}

void DownloadData::execute()
{
     Boolean debug = True;

     BuoyLauncherIF::LC_status l_s;

     BuoyLauncherIF::Filename datafile;

     // Decide what file to download to the buoy
     //
     if (strcmp(_dataFilename, "USE_DEFAULT"))// != NULL)
     {
       char *datadir;
       if ((datadir = getenv("AUV_CONFIG_DIR")) != NULL)
       {
         sprintf(datafile, "%s/%s", datadir, _dataFilename);
       }
       else
       {
         Syslog::write("DownloadData::AUV_CONFIG_DIR undefined");
         setState(Finished);
         return;
       }
     }
     else
     {
       strcpy(datafile, "/usr/include/form.h");
     }

     // Determine whether we should turn-off the periodic auto-get status
     // feature of the launcher driver.
     //
     if (!_statusSet) {
       if (_buoyLauncher->statusOn((Boolean)(_statusCalls != 0) == DeviceIF::Ok))
	 _statusSet = True;
       else
	 _statusSet = False;
     }

     // There are two types of data that the launcher computer can
     // download to the buoy. ARGOS data gets transmitted to the ARGOS
     // satellite, archive data is simply stored on the buoy in hopes
     // that it is recovered and the data off-loaded.
     //
     char typeOfData;
     if (_archiveData > 1)
       typeOfData = 'D';   // Archive data
     else
       typeOfData = 'X';   // ARGOS data

     if( (state() == Active) && 
	 (_legStartTime == NotSpecified) ) {
	  
	  // First starting...
	  _legStartTime = _missionClock->seconds();

     }
     
     if( ( _legDuration != NullLegDuration) && 
	 (_missionClock->seconds() - _legStartTime >= _legDuration) ) {
	  
//	  dprintf("DownloadData::execute - New leg, time = %lf, startTime = %lf, duration = %ld\n", _missionClock->seconds(), _legStartTime, _legDuration );

	  if (++_legsCompleted > _maxLegs) {
	       setState(Finished);
	       return;
	  }
	  
	  // Turn by specified increment and start next leg
	  _heading += _headingIncrement;
	  Math::zeroToTwoPi(&_heading);
	  _legStartTime = _missionClock->seconds();
     }

//     dprintf("DownloadData.cc -- Setting heading = %lf, legs complete = %d\n",
//	     _heading, _legsCompleted );

     setHorizontal(DynamicControlIF::Heading, _heading);
     setSpeed(DynamicControlIF::Speed, _speed);
     

     //** Set vertical mode 
     double vertical = NotSpecified;
     
     switch (_verticalMode) {
	  
     case DynamicControlIF::Depth:
	  vertical = _depth;
	  break;
	  
     case DynamicControlIF::Pitch:
	  vertical = _pitch;
	  break;
	  
     }

     if( vertical != NotSpecified )
	  setVertical(_verticalMode, vertical);
     //
     // Download the data when it's time.
     //
     if( _missionClock->seconds() - _legStartTime >= _downloadDelay ) 
     {
       DeviceIF::Status status;
       //
       // Put check here to determine if the launcher is alive and well.
       //
       if( !_initiateDownload && _buoyLauncher->ready()) 
       {
         // There has to be a responsive buoy in the data position
         // in order to download data.
         //
	 status = _buoyLauncher->getLauncherStatus(&l_s);
	 if (status == DeviceIF::Ok && l_s.buoyResponsive ) {
	   if (_buoyLauncher->loadData(datafile, typeOfData) == DeviceIF::Ok) {
             Syslog::write("DownloadData::Downloading file %s to buoy", datafile);
	     _initiateDownload = True;
	   } else {
             Syslog::write("DownloadData::Buoy Data NOT downloading");
	     _initiateDownload = False;
	   }
	 }
       }
     }

//     Syslog::write("Should check for downloaded data:"
//                   "clock(%lf) - start(%lf) >= delay(%lf) AND "
//                   "_initiateDownload(%d)",
//                   _missionClock->seconds(), _legStartTime, _statusDelay,
//                   _initiateDownload);
 
     // If the download was initiated, check to see if it
     // the file is downloaded to the buoy.
     //
     if( (_missionClock->seconds() - _legStartTime >= _statusDelay) &&
	 _initiateDownload)
     {
       if( !_downloaded &&
	   (DeviceIF::Ok == _buoyLauncher->getLauncherStatus(&l_s)))
       {
         // If nextBuoyReady is true, that means the download is
         // complete.
	 if (l_s.nextBuoyReady)
	 {
	   _downloaded = True;
	   Syslog::write("DownloadData::Download confirmed.\n");
	   setState(Finished);
	 }
       }
     }
     
}


Boolean DownloadData::validInput()
{
  Boolean valid = True;
  Boolean debug = True;

  switch (_verticalMode) {
    
  case DynamicControlIF::Depth:
       if (_depth == NotSpecified || _depth < 0.) {
	    printError("Invalid depth or not specified");
	    valid = False;
       }
       if (_pitchAttribute->valueHasBeenSet()) {
	    dprintf("DownloadData::validInput() - pitch=%.3f, NotSpecified=%.3f", 
		    _pitch, NotSpecified);
	    
	    printError("Pitch specified in depth mode?");
	    valid = False;
       }
       break;
       
  case DynamicControlIF::Pitch:
    //       if (_pitch == NotSpecified || _pitch < 0.) {
       if (_pitch == NotSpecified ) {
	    printError("Invalid pitch or not specified");
	    valid = False;
       }
       if (_depthAttribute->valueHasBeenSet()) {
	    printError("Depth specified in pitch mode?");
	    valid = False;
       }
       break;
       
  case DynamicControlIF::Elevator:
       printError("Elevator mode not implemented");
       valid = False;
       break;
       
  default:
       printError("Note!  Depth not set in DownloadData.  Is this correct?");
       valid = True;
  }

  if (_speed < 0.) {
    printError("Speed is less than zero");
    valid = False;
  }

  if( (_maxLegs > 1) && (_legDuration <= 0.0) ) {
       printError("maxLegs is greater than 1, but legDuration is "
		  "less than zero");
       valid = False;
  }

  if (_maxLegs < 0) {
       printError("maxLegs is less than zero");
       valid = False;
  }

  if( _downloadDelay > _legDuration )
  {
    printError(" The Download Delay must be less than the duration.\n");
    valid = False;
  }

  return valid;
}