//  This class was Automatically Generated on 03/18/2003 @ 22:10:22 from the
//  Component Information file.
//
//  Notes:
//  This namespace is automatically generated using
//  xml2ocp version 2.2 from the following
//  input file: ComponentInfo.xml
//
//  WARNING: Any manual modifications outside of Preserve regions will be
//           overwritten the next time the code generation tool is run.

#include "SimpleTrajectoryGenerator.h"
#include "orbsvcs/Scheduler_Factory.h"

// This is the user specified includes section.  Includes added within the 
// following Preserve Region will be preserved when this component is regenerated.

// User specified includes - Preserve Region
#define PI 3.14159265358979
#include <math.h>
#include "GtmaxLink.h"
#include "esim/quat.h"
double SimpleTrajectoryGenerator::V = 30;
double SimpleTrajectoryGenerator::omega = 0.5;
// User specified includes - End Preserve Region

SimpleTrajectoryGenerator::SimpleTrajectoryGenerator(const char* componentName, const OCP_InputPorts &inputs, const OCP_OutputPorts &outputs)
: OCP_Component_i(componentName, inputs, outputs, NUM_BEHAVIORS)
, stateIn(static_cast<OCP_InputPortT<class GtmaxState>*>(inputs[0]))
, commandOut(static_cast<OCP_OutputPortT<class GtmaxCommand>*>(outputs[0]))
, stateAvailableEnabled_(true)


// This is the user specified initializers section.  Initializers added within the 
// following Preserve Region will be preserved when this component is regenerated.

// User specified initializers - Preserve Region
// User specified initializers - End Preserve Region
{
   stateIn->SetOwningComponent(this);

   this->MapInputPort(StateAvailableId,
                      stateInId,
                      stateIn);


   // This is the user specified initializations section.  Initializations added within the 
   // following Preserve Region will be preserved when this component is regenerated.
   
   // User specified initializations - Preserve Region
   x0 = 0; y0 = 0; z0 = 0; init = 1;
   // User specified initializations - End Preserve Region
}

SimpleTrajectoryGenerator::~SimpleTrajectoryGenerator()
{
   // User specified destruction - Preserve Region
   // User specified destruction - End Preserve Region
}

void SimpleTrajectoryGenerator::RegisterComponent()
{

}

void SimpleTrajectoryGenerator::WireComponent()
{
   ACE_DECLARE_NEW_CORBA_ENV;

   ACE_TRY
   {
      USchedulerTypes::SchedulerFactory::server()->add_dependency(commandOut->GetRT_Info(),
                                                                  stateIn->GetRT_Info(),
                                                                  1,
                                                                  ORB_Scheduler::ONE_WAY_CALL,
                                                                  ACE_TRY_ENV);

      ACE_TRY_CHECK;

   }
   ACE_CATCH(ORB_Scheduler::UNKNOWN_TASK,EX)
   {
      ACE_ERROR((LM_ERROR, "SimpleTrajectoryGenerator::WireComponent (%s) Generated UNKNOWN_TASK Exception during add_dependency call\n"));
   }
   ACE_CATCHANY
   {
      ACE_PRINT_EXCEPTION (ACE_ANY_EXCEPTION, "Unhandled Exception Generated by SimpleTrajectoryGenerator::WireComponent");
      ACE_ERROR((LM_ERROR, "(%t) File: %N, Line: %l, Fatal Exception Enountered During SimpleTrajectoryGenerator::WireComponent\n"));
   }
   ACE_ENDTRY;

}

void SimpleTrajectoryGenerator::Initialize()
{
   GtmaxState* stateInPtr = stateIn->GetInputData();

   char stateInNavStatus = 0;
   char stateInGpsStatus = 0;
   char stateInSonarStatus = 0;
   char stateInRadarStatus = 0;
   char stateInMagnetStatus = 0;
   char stateInOverrun = 0;
   char stateInWow = 0;
   char stateInAutopilot = 0;
   char stateInLavoid = 0;
   double stateInTime = 0.0;
   double stateInX = 0.0;
   double stateInY = 0.0;
   double stateInZ = 0.0;
   double stateInVx = 0.0;
   double stateInVy = 0.0;
   double stateInVz = 0.0;
   double stateInQ1 = 0.0;
   double stateInQ2 = 0.0;
   double stateInQ3 = 0.0;
   double stateInQ4 = 0.0;
   double stateInP = 0.0;
   double stateInQ = 0.0;
   double stateInR = 0.0;
   double stateInRpm = 0.0;
   double stateInAltitudeAGL = 0.0;

   // The following get guarantees coherent data
   stateInPtr->CoherentSet(stateInNavStatus,
                           stateInGpsStatus,
                           stateInSonarStatus,
                           stateInRadarStatus,
                           stateInMagnetStatus,
                           stateInOverrun,
                           stateInWow,
                           stateInAutopilot,
                           stateInLavoid,
                           stateInTime,
                           stateInX,
                           stateInY,
                           stateInZ,
                           stateInVx,
                           stateInVy,
                           stateInVz,
                           stateInQ1,
                           stateInQ2,
                           stateInQ3,
                           stateInQ4,
                           stateInP,
                           stateInQ,
                           stateInR,
                           stateInRpm,
                           stateInAltitudeAGL);

   // Mark the input as available
   if(stateIn->IsFeedback())
   {
      this->MarkInputAvailable(stateInId);

   }


   // This is the user specified code section for the On Method.  Code added within the 
   // following Preserve Region will be preserved when this component is regenerated.
   
   // User specified code - Preserve Region (SimpleTrajectoryGenerator::Initialize)
   // User specified code - End Preserve Region
}

void SimpleTrajectoryGenerator::On(const char* inputs, const char* outputs)
{
   ACE_UNUSED_ARG(inputs);
   ACE_UNUSED_ARG(outputs);

   stateIn->TurnOn();
   commandOut->TurnOn();

   // This is the user specified code section for the On Method.  Code added within the 
   // following Preserve Region will be preserved when this component is regenerated.
   
   // User specified code - Preserve Region (SimpleTrajectoryGenerator::On)
   // User specified code - End Preserve Region
}

void SimpleTrajectoryGenerator::Off(const char* inputs, const char* outputs)
{
   ACE_UNUSED_ARG(inputs);
   ACE_UNUSED_ARG(outputs);

   stateIn->TurnOff();
   commandOut->TurnOff();

   // This is the user specified code section for the Off Method.  Code added within the 
   // following Preserve Region will be preserved when this component is regenerated.
   
   // User specified code - Preserve Region (SimpleTrajectoryGenerator::Off)
   // User specified code - End Preserve Region
}

void SimpleTrajectoryGenerator::StateAvailable()
{
   GtmaxState* stateInPtr = stateIn->GetInputData();

   char stateInNavStatus = 0;
   char stateInGpsStatus = 0;
   char stateInSonarStatus = 0;
   char stateInRadarStatus = 0;
   char stateInMagnetStatus = 0;
   char stateInOverrun = 0;
   char stateInWow = 0;
   char stateInAutopilot = 0;
   char stateInLavoid = 0;
   double stateInTime = 0.0;
   double stateInX = 0.0;
   double stateInY = 0.0;
   double stateInZ = 0.0;
   double stateInVx = 0.0;
   double stateInVy = 0.0;
   double stateInVz = 0.0;
   double stateInQ1 = 0.0;
   double stateInQ2 = 0.0;
   double stateInQ3 = 0.0;
   double stateInQ4 = 0.0;
   double stateInP = 0.0;
   double stateInQ = 0.0;
   double stateInR = 0.0;
   double stateInRpm = 0.0;
   double stateInAltitudeAGL = 0.0;

   // The following get guarantees coherent data
   stateInPtr->CoherentGet(stateInNavStatus,
                           stateInGpsStatus,
                           stateInSonarStatus,
                           stateInRadarStatus,
                           stateInMagnetStatus,
                           stateInOverrun,
                           stateInWow,
                           stateInAutopilot,
                           stateInLavoid,
                           stateInTime,
                           stateInX,
                           stateInY,
                           stateInZ,
                           stateInVx,
                           stateInVy,
                           stateInVz,
                           stateInQ1,
                           stateInQ2,
                           stateInQ3,
                           stateInQ4,
                           stateInP,
                           stateInQ,
                           stateInR,
                           stateInRpm,
                           stateInAltitudeAGL);

   // This is the user specified code section for the SimpleTrajectoryGenerator::StateAvailable.
   // Code added within the following Preserve Region will be preserved when this component is regenerated.

   // User specified code - Preserve Region (SimpleTrajectoryGenerator::StateAvailable)
   /* If this is the first time, grab the current position of helicopter */
   if(init) {
	   x0 = stateInX;
	   y0 = stateInY;
	   z0 = stateInZ;
	   init = 0;
	   ACE_OS::printf("\nStarting manoeuvre x0 = %f, y0 = %f, z0 = %f, V = %f, omega = %f, radius = %f",
		   x0,y0,z0,V,omega,V/omega);
   }

   
   pc[0] = x0 + V/omega*cos(omega*stateInTime);
   pc[1] = y0 + V/omega*sin(omega*stateInTime);
   pc[2] = z0;
   vc[0] = -0*V*sin(omega*stateInTime);
   vc[1] = 0*V*cos(omega*stateInTime);
   vc[2] = 0;
   euler2quat(qc,0,0,omega*stateInTime); /* if 0,0,0 it should produce [1,0,0,0] quaternion */
   wc[0] = 0;
   wc[1] = 0;
   wc[2] = 0;

   

   
   SendCommandOut();
   
   
   // User specified code - End Preserve Region
}


void SimpleTrajectoryGenerator::SendCommandOut()
{
   GtmaxCommand *sigGtmaxCommand = commandOut->GetOutputData();

   // This is the user specified code section for the SimpleTrajectoryGenerator::SendCommandOut Method.
   // Code added within the following Preserve Region will be preserved when this component is regenerated.

   // User specified code - Preserve Region (SimpleTrajectoryGenerator::SendCommandOut)
   commandOut->GetOutputData()->CoherentSet(GTMAX_TRAJ_COMMAND, 
	   pc[0], pc[1], pc[2], 
	   vc[0], vc[1], vc[2], 
	   qc[0], qc[1], qc[2], qc[3], 
	   wc[0], wc[1], wc[2], 
	   0, 0, 0, 0); /* the last four elements are actuator commands (no need to be set in GTMAX_TRAJ_COMMAND mode ) */

   // User specified code - End Preserve Region

   commandOut->OutputDataAvailable();
}



void SimpleTrajectoryGenerator::EnableBehavior(long behaviorId)
{
   switch (behaviorId)
   {
      case StateAvailableId:
         stateAvailableEnabled_ = true;
         break;
   }

}

void SimpleTrajectoryGenerator::DisableBehavior(long behaviorId)
{
   switch (behaviorId)
   {
      case StateAvailableId:
         stateAvailableEnabled_ = false;
         break;
   }

}

void SimpleTrajectoryGenerator::ExecuteBehavior(long behavior_id)
{
   // Clear the latch on all inputs for the given behavior
   this->ResetAllInputs(behavior_id);

   switch (behavior_id)
   {
      case StateAvailableId:
         if (stateAvailableEnabled_)
         {
            StateAvailable();
         }
         break;
      default:
         ACE_ERROR((LM_DEBUG, "%s : Invalid behavior initiated\n", componentName_.c_str()));
         break;
   }

}


// User specified method definitions - Preserve Region
// User specified method definitions - End Preserve Region