#include "VehicleConstants.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"
#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"

#include "Syslog.h"
#include "MathP.h"

//- VehicleConstants (default constructor) --------------------------------
//
VehicleConstants::VehicleConstants( ) 
     : attributes( "VehicleConstants" )
{

     Boolean debug = False;

     _configFileName = NULL;

  dprintf("sizeof(Vector)=%d\n", sizeof(VehicleConfigurationIF::Vector));

  attributes.add(new StringAttribute("vehicleName", "Vehicle name",
				     &_vehicleName));

  attributes.add(new FloatAttribute("propPitch", "Prop pitch?", &_propPitch));
  attributes.add(new FloatAttribute("mass", "vehicle mass", &_mass));

  attributes.add(new FloatAttribute("buoyancy", "Vehicle buoyancy", 
				    &_buoyancy));

  attributes.add(new FloatAttribute("kpHeading", 
				    "Heading controller proportional term",
				    &_kpHeading));

  attributes.add(new FloatAttribute("kdHeading", 
				    "Heading controller differential term",
				    &_kdHeading));

  attributes.add(new FloatAttribute("kiHeading", 
				    "Heading controller integral term",
				    &_kiHeading));

  attributes.add(new AngleAttribute("kiHeadingLimit", 
				    "Heading controller integral term limit?",
				    &_kiHeadingLimit));

  attributes.add(new FloatAttribute("kwpHeading", "?",
				    &_kwpHeading));

  attributes.add(new AngleAttribute("maxHdgRate", "Maximum heading rate",
				    &_maxHdgRate));

  attributes.add(new AngleAttribute("rudLimit", "Rudder limit",
				    &_rudLimit));

  attributes.add(new FloatAttribute("rudActLimit", "Rudder actuator limit",
				    &_rudActLimit));

  attributes.add(new FloatAttribute("rudActRateLimit", 
				    "Rudder actuator rate limit",
				    &_rudActRateLimit));

  attributes.add(new FloatAttribute("kpPitch", 
				    "Pitch controller proportional term",
				    &_kpPitch));

  attributes.add(new FloatAttribute("kdPitch",
				    "Pitch controller differential term",
				    &_kdPitch));

  attributes.add(new FloatAttribute("kiPitch", 
				    "Pitch controller integral term",
				    &_kiPitch));

  attributes.add(new AngleAttribute("kiPitchLimit", 
				    "Pitch controller integral term limit?",
				    &_kiPitchLimit));

  attributes.add(new AngleAttribute("pitchLimit", "Pitch limit",
				    &_pitchLimit));

  attributes.add(new FloatAttribute("maxDiveRate", 
				    "Maximum dive rate",
				    &_maxDiveRate));

  attributes.add(new FloatAttribute("kpDepth", 
				    "Depth controller proportional term",
				    &_kpDepth));

  attributes.add(new FloatAttribute("kiDepth", 
				    "Depth controller integral term",
				    &_kiDepth));

  attributes.add(new AngleAttribute("elevLimit", 
				    "Elevator limit",
				    &_elevLimit));

  attributes.add(new FloatAttribute("elevActLimit", 
				    "Elevator actuator limit",
				    &_elevActLimit));

  attributes.add(new FloatAttribute("elevActRateLimit", 
				    "Elevator actuator rate limit",
				    &_elevActRateLimit));

  attributes.add(new FloatAttribute("Ts", "Simulator timestep", &_ts, 0.2));

  attributes.add(new FloatAttribute("effDragCoef", 
				    "Effective drag coefficient",
				    &_effDragCoef));

  attributes.add(new FloatAttribute("tau", "?", &_tau));

  attributes.add(new FloatAttribute("ductArea", "?", &_ductArea));

  attributes.add(new AngleAttribute("maxDepthInt", "?",
				    &_maxDepthInt));

  attributes.add(new AngleAttribute("maxHdgInt", "?", &_maxHdgInt));

  attributes.add(new AngleAttribute("maxPitchInt", "?", &_maxPitchInt));

  attributes.add(new FloatAttribute("Xuabu", "?", &_Xuabu));

  attributes.add(new FloatAttribute("decouple", "0.", &_decouple));

  attributes.add(new FloatAttribute("altimeterInstrument", 
				    "0=> DVL, 1=> PSA", 
				    &_altimeterInstrument));

  attributes.add(new FloatAttribute("useLbl", 
				    "Use the LBL for navigation", 
				    &_useLbl));
  attributes.add(new FloatAttribute("useIns", 
				    "Use the Ins for navigation", 
				    &_useIns));

  attributes.add(new FloatAttribute("propEfficiency", "?", &_propEfficiency));

  attributes.add(new FloatAttribute("centerOfMassX", 
				    "Center of mass X",
				    &_centerOfMass[0]));

  attributes.add(new FloatAttribute("centerOfMassY", 
				    "Center of mass Y",
				    &_centerOfMass[1]));

  attributes.add(new FloatAttribute("centerOfMassZ", 
				    "Center of mass Z",
				    &_centerOfMass[2]));

  attributes.add(new FloatAttribute("centerOfBuoyX", 
				    "Center of buoyancy X",
				    &_centerOfBuoyancy[0]));

  attributes.add(new FloatAttribute("centerOfBuoyY", 
				    "Center of buoyancy Y",
				    &_centerOfBuoyancy[1]));

  attributes.add(new FloatAttribute("centerOfBuoyZ", 
				    "Center of buoyancy Z",
				    &_centerOfBuoyancy[2]));

  attributes.add(new FloatAttribute("lowerRudX", 
				    "Lower rudder X",
				    &_lowerRudderLoc[0]));

  attributes.add(new FloatAttribute("lowerRudY", 
				    "Lower rudder Y",
				    &_lowerRudderLoc[1]));

  attributes.add(new FloatAttribute("lowerRudZ", 
				    "Lower rudder Z",
				    &_lowerRudderLoc[2]));

  attributes.add(new FloatAttribute("upperRudX", 
				    "Upper rudder X",
				    &_upperRudderLoc[0]));

  attributes.add(new FloatAttribute("upperRudY", 
				    "Upper rudder Y",
				    &_upperRudderLoc[1]));

  attributes.add(new FloatAttribute("upperRudZ", 
				    "Upper rudder Z",
				    &_upperRudderLoc[2]));

  attributes.add(new FloatAttribute("portElevX", 
				    "Port elevator X",
				    &_portElevatorLoc[0]));

  attributes.add(new FloatAttribute("portElevY", 
				    "Port elevator Y",
				    &_portElevatorLoc[1]));

  attributes.add(new FloatAttribute("portElevZ", 
				    "Port elevator Z",
				    &_portElevatorLoc[2]));

  attributes.add(new FloatAttribute("stbdElevX", 
				    "Stbd elevator X",
				    &_stbdElevatorLoc[0]));

  attributes.add(new FloatAttribute("stbdElevY", 
				    "Stbd elevator Y",
				    &_stbdElevatorLoc[1]));

  attributes.add(new FloatAttribute("stbdElevZ", 
				    "Stbd elevator Z",
				    &_stbdElevatorLoc[2]));

}

//- ~VehicleConstants (default destructor) --------------------------------
//
VehicleConstants::~VehicleConstants()
{
     if( _configFileName  != NULL )
	  delete _configFileName;
}

//- loadConstants ---------------------------------------------------------
//
int VehicleConstants::loadConstants( const char *confFileName) 
{

     try {
	  AttributeParser::parse(confFileName, &attributes);
     }
     catch(...)
     {
	  Syslog::write("VehicleConstants -- Caught exception while "
			   "loading vehicle constants");
	  return -1;
     }

     _configFileName = strdup(confFileName);

     initDropWeights();

     return 0;

}

//=========================================================================
//
//  Dropweight functions
//
//

void VehicleConstants::initDropWeights()
{

  // Hard-code Odyssey dropweight characteristics for now!
  VehicleConstants::DropWeight *dropWeight;

  dropWeight = new VehicleConstants::DropWeight();
  dropWeight->mass = 10.;
  dropWeight->buoyancy = 0.;
  dropWeight->location[X] = 0.1655;
  dropWeight->location[Y] = dropWeight->location[Z] = 0.;

  _dropWeights.add(&dropWeight);

  dropWeight = new VehicleConstants::DropWeight();
  dropWeight->mass = 10.;
  dropWeight->buoyancy = 0.;
  dropWeight->location[X] = 0.1655;
  dropWeight->location[Y] = dropWeight->location[Z] = 0.;
  
  _dropWeights.add(&dropWeight);
}

short VehicleConstants::nDropWeights()
{
  return (short )_dropWeights.size();
}