//  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 "GtmaxLink.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
/*** BeginCopyright
 * Copyright 2002, Georgia Institute of Technology, All Rights Reserved.
 * Unauthorized use and/or redistribution is disallowed.
 * This library is distributed without any warranty; without even
 * the implied warranty of fitness for a particular purpose.
 *
 * UAV Laboratory
 * School of Aerospace Engineering
 * Georgia Institute of Technology
 * Atlanta, GA 30332
 * http://controls.ae.gatech.edu/uavrf
 *
 * Contact Information:
 * Prof. Eric N. Johnson
 * http://controls.ae.gatech.edu
 * Tel : 404 385 2519
 * EndCopyright
 ***/
/***
 * Header       : $Header: /home/cvs/cvsroot/gtmaxocp/ComponentDatabase/GtmaxLink.cpp,v 1.5 2003/03/19 02:54:03 kannan Exp $ 
 * Authors      : Kannan, Gutierrez
 * LastModified : $Author: kannan $
 * Purpose      : An OCP component that serves as an interface to the GTMax helicopter
 *              : Do not modify this component, any change requests should be made
 *              : to the uavlab through suresh_kannan@ae.gatech.edu or eric.johnson@ae.gatech.edu
 ***/
//********************* My code *********************
#include "esim/util.h"
#include "rmax/rmaxconfig.h"
#include "rmax/serial.h"
#include "rmax/onboard2_ref.h"
#include "rmax/onboard2.h"
#include "esim/quat.h"
//***************** End of my code ******************
// User specified includes - End Preserve Region

GtmaxLink::GtmaxLink(const char* componentName, const OCP_InputPorts &inputs, const OCP_OutputPorts &outputs)
: OCP_Component_i(componentName, inputs, outputs, NUM_BEHAVIORS)
, triggerIn(inputs[0])
, commandIn(static_cast<OCP_InputPortT<class GtmaxCommand>*>(inputs[1]))
, stateOut(static_cast<OCP_OutputPortT<class GtmaxState>*>(outputs[0]))
, triggerAvailableEnabled_(true)
, commandAvailableEnabled_(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
{
   triggerIn->SetOwningComponent(this);
   commandIn->SetOwningComponent(this);

   this->MapInputPort(TriggerAvailableId,
                      triggerInId,
                      triggerIn);

   this->MapInputPort(CommandAvailableId,
                      commandInId,
                      commandIn);


   // 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
   // User specified initializations - End Preserve Region
}

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

void GtmaxLink::RegisterComponent()
{

}

void GtmaxLink::WireComponent()
{
   ACE_DECLARE_NEW_CORBA_ENV;

   ACE_TRY
   {
      USchedulerTypes::SchedulerFactory::server()->add_dependency(stateOut->GetRT_Info(),
                                                                  triggerIn->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, "GtmaxLink::WireComponent (%s) Generated UNKNOWN_TASK Exception during add_dependency call\n"));
   }
   ACE_CATCHANY
   {
      ACE_PRINT_EXCEPTION (ACE_ANY_EXCEPTION, "Unhandled Exception Generated by GtmaxLink::WireComponent");
      ACE_ERROR((LM_ERROR, "(%t) File: %N, Line: %l, Fatal Exception Enountered During GtmaxLink::WireComponent\n"));
   }
   ACE_ENDTRY;

}

void GtmaxLink::Initialize()
{
   GtmaxCommand* commandInPtr = commandIn->GetInputData();

   int commandInType = 0;
   double commandInX = 0.0;
   double commandInY = 0.0;
   double commandInZ = 0.0;
   double commandInVx = 0.0;
   double commandInVy = 0.0;
   double commandInVz = 0.0;
   double commandInQ1 = 0.0;
   double commandInQ2 = 0.0;
   double commandInQ3 = 0.0;
   double commandInQ4 = 0.0;
   double commandInP = 0.0;
   double commandInQ = 0.0;
   double commandInR = 0.0;
   double commandInDelm1 = 0.0;
   double commandInDelm2 = 0.0;
   double commandInDelm3 = 0.0;
   double commandInDelf = 0.0;

   // The following get guarantees coherent data
   commandInPtr->CoherentSet(commandInType,
                             commandInX,
                             commandInY,
                             commandInZ,
                             commandInVx,
                             commandInVy,
                             commandInVz,
                             commandInQ1,
                             commandInQ2,
                             commandInQ3,
                             commandInQ4,
                             commandInP,
                             commandInQ,
                             commandInR,
                             commandInDelm1,
                             commandInDelm2,
                             commandInDelm3,
                             commandInDelf);

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

   }


   // 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 (GtmaxLink::Initialize)
   // User specified code - End Preserve Region
}

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

   triggerIn->TurnOn();
   commandIn->TurnOn();
   stateOut->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 (GtmaxLink::On)
   //********************* My code *********************
   	onboard2.init = 1;
	onboard2PortIPC.dataSource = PORT_ON;
   //***************** End of my code ******************

   // User specified code - End Preserve Region
}

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

   triggerIn->TurnOff();
   commandIn->TurnOff();
   stateOut->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 (GtmaxLink::Off)
   // User specified code - End Preserve Region
}

void GtmaxLink::TriggerAvailable()
{
   // This is the user specified code section for the GtmaxLink::TriggerAvailable.
   // Code added within the following Preserve Region will be preserved when this component is regenerated.

   // User specified code - Preserve Region (GtmaxLink::TriggerAvailable)
   //********************* My code *********************
	struct onboard2_ref    *ob2   = &onboard2;
	struct ob2Datalink_ref *data = ob2->datalink;
	if( ob2->init ) {
		data->work->lastUpdate0 = -data->work->updateDt0;
	}


	/* reset any packet received flags */
	ob2->ipc0PktRecvd = 0;
	ob2->sendCommand = 0;
	ob2->sendActuator = 0;

	/* read ICL */

	readDatalinkOb2( ob2, data->p );

	/* send gtmax state out to those who want it */
	if( ob2->ipc0PktRecvd ) {
		SendStateOut();
	}
   //***************** End of my code ******************
   // User specified code - End Preserve Region
}

void GtmaxLink::CommandAvailable()
{
   GtmaxCommand* commandInPtr = commandIn->GetInputData();

   int commandInType = 0;
   double commandInX = 0.0;
   double commandInY = 0.0;
   double commandInZ = 0.0;
   double commandInVx = 0.0;
   double commandInVy = 0.0;
   double commandInVz = 0.0;
   double commandInQ1 = 0.0;
   double commandInQ2 = 0.0;
   double commandInQ3 = 0.0;
   double commandInQ4 = 0.0;
   double commandInP = 0.0;
   double commandInQ = 0.0;
   double commandInR = 0.0;
   double commandInDelm1 = 0.0;
   double commandInDelm2 = 0.0;
   double commandInDelm3 = 0.0;
   double commandInDelf = 0.0;

   // The following get guarantees coherent data
   commandInPtr->CoherentGet(commandInType,
                             commandInX,
                             commandInY,
                             commandInZ,
                             commandInVx,
                             commandInVy,
                             commandInVz,
                             commandInQ1,
                             commandInQ2,
                             commandInQ3,
                             commandInQ4,
                             commandInP,
                             commandInQ,
                             commandInR,
                             commandInDelm1,
                             commandInDelm2,
                             commandInDelm3,
                             commandInDelf);

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

   // User specified code - Preserve Region (GtmaxLink::CommandAvailable)
   //********************* My code *********************
   struct onboard2_ref    *ob2   = &onboard2;
   struct ob2Datalink_ref *data = ob2->datalink;
   struct datalinkMessageIPC0_ref  *ipc0  = ob2->datalink->ipc0;
   struct datalinkMessageCommand_ref *cmd = ob2->datalink->command;
   struct datalinkMessageUp0_ref *up0     = ob2->datalink->up0;
   
   
   switch(commandInType) {
   case GTMAX_TRAJ_COMMAND:
	   
	   cmd->x[0] = commandInX;
	   cmd->x[1] = commandInY;
	   cmd->x[2] = commandInZ;
	   cmd->v[0] = commandInVx;
	   cmd->v[1] = commandInVy;
	   cmd->v[2] = commandInVz;
	   cmd->q[0] = commandInQ1;
	   cmd->q[1] = commandInQ2;
	   cmd->q[2] = commandInQ3;
	   cmd->q[3] = commandInQ4;
	   cmd->w[0] = commandInP;
	   cmd->w[1] = commandInQ;
	   cmd->w[2] = commandInR;
	   
	   ob2->sendCommand = 1;
	   writeDatalinkOb2( ob2, data->p );
	   break;
   case GTMAX_ACTUATOR_COMMAND:
	   up0->delm[0] = commandInDelm1;
	   up0->delm[1] = commandInDelm2;
	   up0->delm[2] = commandInDelm3;
	   up0->delf[0] = commandInDelf;

	   ob2->sendActuator = 1;
	   writeDatalinkOb2( ob2, data->p );
	   break;
   default:
	   break;
   };

   //***************** End of my code ******************
   // User specified code - End Preserve Region
}


void GtmaxLink::SendStateOut()
{
   GtmaxState *sigGtmaxState = stateOut->GetOutputData();

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

   // User specified code - Preserve Region (GtmaxLink::SendStateOut)
   //********************* My code *********************
   struct onboard2_ref    *ob2   = &onboard2;
   struct ob2Datalink_ref *data = ob2->datalink;
   struct datalinkMessageIPC0_ref  *ipc0  = ob2->datalink->ipc0;

   sigGtmaxState->CoherentSet(ipc0->navStatus, ipc0->gpsStatus, ipc0->sonarStatus, ipc0->radarStatus, ipc0->magnetStatus,
	   ipc0->overrun, ipc0->wow, ipc0->autopilot, ipc0->lavoid, ipc0->time,ipc0->pos[0],ipc0->pos[1],ipc0->pos[2],
	   ipc0->vel[0],ipc0->vel[1],ipc0->vel[2], ipc0->q[0], ipc0->q[1], ipc0->q[2], ipc0->q[3], ipc0->w[0], ipc0->w[1], ipc0->w[2],
	   ipc0->rpm, ipc0->altitudeAGL);

   //***************** End of my code ******************
   // User specified code - End Preserve Region

   stateOut->OutputDataAvailable();
}



void GtmaxLink::EnableBehavior(long behaviorId)
{
   switch (behaviorId)
   {
      case TriggerAvailableId:
         triggerAvailableEnabled_ = true;
         break;
      case CommandAvailableId:
         commandAvailableEnabled_ = true;
         break;
   }

}

void GtmaxLink::DisableBehavior(long behaviorId)
{
   switch (behaviorId)
   {
      case TriggerAvailableId:
         triggerAvailableEnabled_ = false;
         break;
      case CommandAvailableId:
         commandAvailableEnabled_ = false;
         break;
   }

}

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

   switch (behavior_id)
   {
      case TriggerAvailableId:
         if (triggerAvailableEnabled_)
         {
            TriggerAvailable();
         }
         break;
      case CommandAvailableId:
         if (commandAvailableEnabled_)
         {
            CommandAvailable();
         }
         break;
      default:
         ACE_ERROR((LM_DEBUG, "%s : Invalid behavior initiated\n", componentName_.c_str()));
         break;
   }

}


// User specified method definitions - Preserve Region
void GtmaxLink::SetFlightComputerName(const char *name) {
	strcpy(onboard2PortIPC.connectTo,name);
}
// User specified method definitions - End Preserve Region