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

#include "VehicleConfigurationServer.h"


VehicleConfigurationServer::VehicleConfigurationServer(const char *name,
						       const char *filename)
  : VehicleConfigurationIF_SK()
{
  Boolean debug = False;

  // -- Start by loading vehicular constants ---------------
  _constants = NULL;
  try {
       _constants = new VehicleConstants();
  }
  catch(...)
  {
       Syslog::write("VehicleConfigurationServer -- Caught exception while "
		     "initializing vehicle constants");
  }
  if( _constants->loadConstants( filename ) )
  {
       Syslog::write("VehicleConfigurationServer -- Failed to load vehicle "
		     "constants.  Exiting...");
       exit(-1);
  }

  // -- Then take care of device config -------------------
  _devList = NULL;
  try {
       _devList = new DeviceList( );
  }
  catch(...)
  {
       Syslog::write("VehicleConfigurationServer -- Caught exception while "
		     "initializing device list");
  }


  /*
    Not yet sure how to load devices... are they REALLY part of 
    VehicleConfigurationServer? And how do they play with devices.cfg????
    T.O'R.
    */
#if 0
  if( _devList->loadDevices( "dev.cfg" ) )
  {
       Syslog::write("VehicleConfigurationServer -- Failed to load device "
		     "table.  Exiting...");
       exit(-1);
  }
#endif

}


VehicleConfigurationServer::~VehicleConfigurationServer()
{
     if( _constants != NULL ) delete _constants;

     if( _devList != NULL ) delete _devList;
	
}

//=========================================================================
//

//- deviceCount() ----------------------------------------
short VehicleConfigurationServer::deviceCount()
{
     return _devList->count();
}

//- findDevice() -----------------------------------------
short VehicleConfigurationServer::findDevice( VehicleConfigurationIF::Name name )
{
     return _devList->findItemIndex( name );
}

//- getDeviceDriver() ------------------------------------
short VehicleConfigurationServer::getDeviceDriver(
     short index,
     VehicleConfigurationIF::Driver driver )
{
     Device *current;
     if( (current=(Device *)_devList->findItem( index )) == NULL )
	  return -1;

     strcpy( driver, current->driver() );
     return 0;
}
short VehicleConfigurationServer::getDeviceDriver(
     VehicleConfigurationIF::Name name,
     VehicleConfigurationIF::Driver driver )
{
     Device *current;
     if( (current=(Device *)_devList->findItem( name )) == NULL )
	  return -1;
     
     strcpy( driver, current->driver() );
     return 0;
}

//- getDeviceDriver() ------------------------------------
short VehicleConfigurationServer::getDeviceName(
     short index,
     VehicleConfigurationIF::Name name )
{
     Device *current;
     if( (current=(Device *)_devList->findItem( index )) == NULL )
	  return -1;

     strcpy( name, current->name() );
     return 0;
}

//- deviceInfo() -----------------------------------------
short VehicleConfigurationServer::deviceInfo(short index, 
					     VehicleConfigurationIF::Name name, 
					     VehicleConfigurationIF::Port port, 
					     short *priority)
{
     Boolean debug = True;
     Device *current;

     if( ( current = (Device *) _devList->findItem( index ) ) == NULL )
     {
	  dprintf("VehicleConfigurationSerer::deviceInfo - Couldn't find a device named %s",
		  name );
	  return -1;
     }
     
     strcpy( name, current->name() );
     strcpy( port, current->port() );
     *priority = current->priority();

     return 0;
     
}

//- deviceInfo() -----------------------------------------
short VehicleConfigurationServer::deviceInfo(VehicleConfigurationIF::Name name, 
			  VehicleConfigurationIF::Port port, short *priority)
{
     Boolean debug = True;
     Device *current;

     if( ( current = (Device *) _devList->findItem( name ) ) == NULL )
     {
	  dprintf("VehicleConfigurationSerer::deviceInfo - Couldn't find a device named %s",
		  name );
	  return -1;
     }
     
     strcpy( port, current->port() );
     *priority = current->priority();

     return 0;
}


void VehicleConfigurationServer::vehicleName(VehicleConfigurationIF::Name name)
{
  strcpy(name, _constants->_vehicleName);
}


double VehicleConfigurationServer::propPitch()
{
  return _constants->_propPitch;
}


double VehicleConfigurationServer::mass()
{
  return _constants->_mass;
}


double VehicleConfigurationServer::buoyancy()
{
  return _constants->_buoyancy;
}


double VehicleConfigurationServer::kpHeading()
{
  return _constants->_kpHeading;
}


double VehicleConfigurationServer::kdHeading()
{
  return _constants->_kdHeading;
}


double VehicleConfigurationServer::kiHeading()
{
  return _constants->_kiHeading;
}


double VehicleConfigurationServer::kiHeadingLimit()
{
  return _constants->_kiHeadingLimit;
}


double VehicleConfigurationServer::kwpHeading()
{
  return _constants->_kwpHeading;
}


double VehicleConfigurationServer::maxHdgRate()
{
  return _constants->_maxHdgRate;
}


double VehicleConfigurationServer::rudLimit()
{
  return _constants->_rudLimit;
}


double VehicleConfigurationServer::rudActLimit()
{
  return _constants->_rudActLimit;
}


double VehicleConfigurationServer::rudActRateLimit()
{
  return _constants->_rudActRateLimit;
}


double VehicleConfigurationServer::kpPitch()
{
  return _constants->_kpPitch;
}

double VehicleConfigurationServer::kdPitch()
{
  return _constants->_kdPitch;
}


double VehicleConfigurationServer::kiPitch()
{
  return _constants->_kiPitch;
}


double VehicleConfigurationServer::kiPitchLimit()
{
  return _constants->_kiPitchLimit;
}


double VehicleConfigurationServer::pitchLimit()
{
  return _constants->_pitchLimit;
}


double VehicleConfigurationServer::maxDiveRate()
{
  return _constants->_maxDiveRate;
}


double VehicleConfigurationServer::kpDepth()
{
  return _constants->_kpDepth;
}


double VehicleConfigurationServer::kiDepth()
{
  return _constants->_kiDepth;
}


double VehicleConfigurationServer::propEfficiency()
{
  return _constants->_propEfficiency;
}


double VehicleConfigurationServer::elevLimit()
{
  return _constants->_elevLimit;
}


double VehicleConfigurationServer::elevActLimit()
{
  return _constants->_elevActLimit;
}


double VehicleConfigurationServer::elevActRateLimit()
{
  return _constants->_elevActRateLimit;
}


double VehicleConfigurationServer::Ts()
{
  return _constants->_ts;
}


double VehicleConfigurationServer::effDragCoef()
{
  return _constants->_effDragCoef;
}


double VehicleConfigurationServer::tau()
{
  return _constants->_tau;
}


double VehicleConfigurationServer::maxDepthInt()
{
  return _constants->_maxDepthInt;
}


double VehicleConfigurationServer::maxHdgInt()
{
  return _constants->_maxHdgInt;
}


double VehicleConfigurationServer::maxPitchInt()
{
  return _constants->_maxPitchInt;
}


int VehicleConfigurationServer::spawnAuxTasks()
{
  return 0;
}


void VehicleConfigurationServer::centerOfMass(VehicleConfigurationIF::Vector v)
{
  memcpy((void *)v, (void *)_constants->_centerOfMass, 
	 sizeof(VehicleConfigurationIF::Vector));
}


void VehicleConfigurationServer::centerOfBuoyancy(VehicleConfigurationIF::Vector v)
{
  memcpy((void *)v, (void *)_constants->_centerOfBuoyancy,
	 sizeof(VehicleConfigurationIF::Vector));
}


void VehicleConfigurationServer::finLocations(VehicleConfigurationIF::Vector 
					      lowerRudder, 

					      VehicleConfigurationIF::Vector 
					      upperRudder, 

					      VehicleConfigurationIF::Vector 
					      portElevator, 

					      VehicleConfigurationIF::Vector 
					      stbdElevator)
{
  memcpy((void *)lowerRudder, (void *)_constants->_lowerRudderLoc,
	 sizeof(VehicleConfigurationIF::Vector));

  memcpy((void *)upperRudder, (void *)_constants->_upperRudderLoc,
	 sizeof(VehicleConfigurationIF::Vector));

  memcpy((void *)portElevator, (void *)_constants->_portElevatorLoc,
	 sizeof(VehicleConfigurationIF::Vector));

  memcpy((void *)stbdElevator, (void *)_constants->_stbdElevatorLoc,
	 sizeof(VehicleConfigurationIF::Vector));
}


double VehicleConfigurationServer::ductArea()
{
  return _constants->_ductArea;
}


short VehicleConfigurationServer::nDropWeights()
{
  return (short )_constants->_dropWeights.size();
}


short VehicleConfigurationServer::dropWeight(short weightNo, 
					    double *mass,
					    double *buoyancy,
					    VehicleConfigurationIF::Vector
					    location)
{
  if (weightNo < 0 || weightNo >= _constants->_dropWeights.size()) {
    return -1;
  }

  VehicleConstants::DropWeight *dropWeight;
  _constants->_dropWeights.get(weightNo, &dropWeight);

  *mass = dropWeight->mass;
  *buoyancy = dropWeight->buoyancy;

  memcpy(location, dropWeight->location, 
	 sizeof(VehicleConfigurationIF::Vector));

  return 0;
}


double VehicleConfigurationServer::xuabu()
{
  return _constants->_Xuabu;
}

double VehicleConfigurationServer::decouple()
{
  return _constants->_decouple;
}

double VehicleConfigurationServer::altimeterInstrument()
{
  return _constants->_altimeterInstrument;
}

double VehicleConfigurationServer::useLbl()
{
  return _constants->_useLbl;
}

double VehicleConfigurationServer::useIns()
{
  return _constants->_useIns;
}