//=========================================================================
// Summary  : 
// Filename : VehicleConstants.cc
// Author   : marsh
// Project  : 
// Revision : 1
// Created  : 2000/08/19
// Modified : 2000/08/19
//=========================================================================
// Description :
//=========================================================================

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

#include "Syslog.h"
#include "Math.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("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]));
    
    attributes.add(new FloatAttribute("highModePercent",
				      "Percent for AvoidMode High",
				      &_highModePercent, 95));
    
    attributes.add(new FloatAttribute("lowModePercent",
				      "Percent for AvoidMode Low",
				      &_lowModePercent, 55));
  
}

//- ~VehicleConstants (default destructor) --------------------------------
//
VehicleConstants::~VehicleConstants()
{
     clean( _configFileName );
     clean( _vehicleName );
     VehicleConstants::DropWeight *dropWeight;
     for (int i = 0; i < _dropWeights.size(); i++) {    
	 _dropWeights.get(i, &dropWeight);
	 delete dropWeight;
     }
}

//- loadConstants ---------------------------------------------------------
//
int VehicleConstants::loadConstants( const char *confFileName) 
{
    
     try {
	  AttributeParser::parse(confFileName, &attributes);
     }
     catch(...)
     {
	  Syslog::write("VehicleConstants -- Caught exception while "
			   "loading vehicle constants");
	  return -1;
     }
    
     clean( _configFileName );
     _configFileName = strdup(confFileName);
     
     initDropWeights();
     
     return 0;

}

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

void VehicleConstants::initDropWeights()
{
    // Hard-code Odyssey dropweight characteristics for now!
    VehicleConstants::DropWeight *dropWeight;
    
    for (int i = 0; i < _dropWeights.size(); i++) {    
	_dropWeights.get(i, &dropWeight);
	delete dropWeight;
    }
    _dropWeights.clear();
    
    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();
}