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

const int callbackPeriod_ms = 100;

const int elevPeriod_ms = 8000;
const double elevAmp_deg = 30;

const int rudPeriod_ms = 12000;
const double rudAmp_deg = 30;

WiggleTest::WiggleTest() :
     PeriodicTask("wiggleTest")
{

     try {
	  _tailcone = new TailConeIF("tailCone",500);
     }
     catch( SharedObjectClient::MissingServer e) {

	  printf("WiggelTest.cc -- No TailConeIF server found: %s\n",
		 e.msg );
     }
     catch( ... ) {
	  printf("WiggleTest.cc -- Exception on TailConeIF\n");
     }

     StartTime_ms = -1;

     rudAmp_rad = Math::degToRad( rudAmp_deg );
     elevAmp_rad = Math::degToRad( elevAmp_deg );

     addPeriodicCallback( callbackPeriod_ms, 
			  (CallbackMethod)WiggleTest::Callback );

}

WiggleTest::~WiggleTest()
{
     delete _tailcone;
}

void WiggleTest::Callback()
{
     double dt, rudAngle_rad, elevAngle_rad; 

     if( StartTime_ms < 0 )
	  StartTime_ms = Time::milliseconds();

     dt = Time::milliseconds() - StartTime_ms;

     rudAngle_rad = rudAmp_rad * sin( (dt/rudPeriod_ms) * 2.0 * PI );
     elevAngle_rad = elevAmp_rad * sin( (dt/elevPeriod_ms) * 2.0 * PI );

//     printf("WiggleTest.cc -- Setting r = %lf, e = %lf\n",
//	    Math::radToDeg( rudAngle_rad ), Math::degToRad( elevAngle_rad ) );

     try {
	  _tailcone->command( 0, elevAngle_rad, rudAngle_rad );
     }
     catch(...)
     {
	  printf("WiggleTest.cc -- _tailcone->command failed\n");
     }

}