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

ChirpFin::ChirpFin()
  : Behavior(ChirpFinBehaviorName, NonSequential)
{
  attributes.add(new FloatAttribute("amplitude", "Amplitude of sine wave",
				    &_amplitude));

  attributes.add(new FloatAttribute("initialFreq", 
				    "Initial sine wave frequency", 
				    &_initialFrequency));

  attributes.add(new FloatAttribute("finalFreq", 
				    "Final sine wave frequency",
				    &_finalFrequency));

  attributes.add(new StringAttribute("plane", 
				     "Either \"horizontal\" or \"vertical\"",
				     &_planeMnem));
}


ChirpFin::~ChirpFin()
{
}


void ChirpFin::execute()
{

  time_t t = _missionClock->seconds() - _startTime;

  double freq = _initialFrequency + 
    (_finalFrequency - _initialFrequency) * t / (endTime() - startTime());

  double angle = _amplitude * sin(2. * M_PI * freq * t);

  if (_plane == HorizontalPlane) 
    setHorizontal(DynamicControlIF::Rudder, angle);
  else 
    setVertical(DynamicControlIF::Elevator, angle);

  return;
}


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

  if (_amplitude < 0.) {
    printError("Negative amplitude; must be positive");
    valid = False;
  }

  if (_initialFrequency < 0. || _finalFrequency < 0.) {
    printError("Negative frequency; must be positive");
    valid = False;
  }

  if (!strcmp(_planeMnem, "horizontal")) {
    _plane = HorizontalPlane;
  }
  else if (!strcmp(_planeMnem, "vertical")) {
    _plane = VerticalPlane;
  }
  else {
    printError("Invalid plane; must be \"horizontal\" or \"vertical\"");
    valid = False;
  }

  return valid;
}