/****************************************************************************
*   File: trk_rao.c
*                                                                           *
*       Copyright 1996 by Loral Advanced Distributed Simulation, Inc.       *
*                                                                           *
*               Loral Advanced Distributed Simulation, Inc.                 *
*               50 Moulton Street                                           *
*               Cambridge, MA 02138                                         *
*               617-441-2000                                                *
*                                                                           *
*       This software was developed by Loral under U. S. Government contracts *
*       and may be reproduced by or for the U. S. Government pursuant to    *
*       the copyright license under the clause at DFARS 252.227-7013        *
*       (OCT 1988).                                                         *
*                                                                           *
*       Contents: Response Amplitude Operators for ocean wave interaction
*       Created: Sun Sep  1 18:07:38 EDT 1996
*       Author: skukolic
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

/* For organizational purposes, this file is split into 4 sections: */
/* INCLUDES AND DECLARATIONS: */
/* READER FILE STUFF */
/* MODSAF INTERFACE */
/* THIS RAO MODEL: */
/* RAO MODEL TEST PROGRAM: */

/**************************************************************/
/**************************************************************/

/* INCLUDES AND DECLARATIONS: */

/**************************************************************/

#include <math.h>

#ifdef TRK_RAO_TEST
#define TRACKED_RAO_TEST
#endif /*TRK_RAO_TEST*/

#ifdef TRACKED_RAO_TEST
#ifndef NO_MODSAF
#define NO_MODSAF
#endif /*not NO_MODSAF*/
#endif /*TRACKED_RAO_TEST*/

#ifndef NO_MODSAF

#ifdef NO_READER
#undef NO_READER
#endif

#define STATIC  static

/* ModSAF includes */

#include "libtrk_local.h"
#include <sys/time.h>
#include <libreader.h>
#include <libenvironment.h>
#include <libenvsea.h>
#include <libenvcloud.h> /* for EnvXYZ coords */
#include <libphysdb.h>
#include <libentity.h>
#include <libvecmat.h>
#include "veh_type.h"

#endif /*not NO_MODSAF*/


#ifdef NO_MODSAF
/* need types so that we can compile this file standalone*/

/* STATIC identifies public functions for non-ModSAF Apps */
#define STATIC

#ifndef NO_READER

#include <stdtypes.h>
#include <libreader.h>

#else

typedef int    int32;
typedef float  float32;
typedef double float64;

#endif

#ifndef PI
#define PI 3.14159265358979323844
#endif

#endif /*NO_MODSAF*/


/**************************************************************/
/* local RAO model defines, and structs */

/* physical constants: */
#define WATER_DENSITY 1000.0 /* kg/m^3 */
#define GRAVITY       9.8    /* m/s^2 */
#define DEFAULT_RAO_DAMP_CONSTANT  (0.3)

/* Hull directions for which we will calculate ship's amplitude and phase
 * response to each input wave frequency. */
typedef enum {
    RAO_Z, 
    RAO_PITCH, /* angle about X axis, X points right out of ship */
    RAO_ROLL,  /* angle about Y axis, Y axis points out of front of ship */
    N_RAO_DIM
} RAO_DIM;

typedef struct {
    float64  w0, damp; 
    /* damp is very roughly the amplitude lost fraction per cycle,
     * friction coeff b = damp*w0/Pi, this properly defines damp
     * pole_response(w) = 1/((-I*w)**2 + (-I*w)*b + w0**2)
     *                  = (w0**2 - w**2 + I*w*b)
     *                    /((w0**2 - w**2)**2 + (w*b)**2).    */
} RAO_POLE;

typedef struct {
    /* dims */
    float64  mass, len[3]; /* kg, meters */
    float64  waterline;    /* height from ship bottom */
    /* poles */
    RAO_POLE rao_poles[N_RAO_DIM]; /* for getting amplitude and phase
                                    * response to driving frequency */
    /* inertias */
    float64  inertias[N_RAO_DIM];  /* mass, and moments of inertia */
    /* pos, vel */
    float64  pos[3], vel[3], y_direction[2]; /* inputs describing ship state*/
} RAO_VEH;

/**************************************************************/
/* utility definitions used by this RAO model */

/* for holding and calculating wave spectra: */

typedef struct 
{
    float32	frequency;
    float32	amplitude;
    float32	phase;
    float32	wave_number[2];
} SEA_SPECTRAL_DATA_REC;

/* for use with a function which iterates over the wave spectra: */

typedef struct
{
    float64  pos0[4];
    int32  nwaves, nfreqs, iwave, ifreq;
} SPECTRA_ITER;


/* COMPLEX number arithmetic needed in RAO calculations */

typedef struct
{
    float64  re, im;
}  COMPLEX;

#define complex_real_part(C) ((C).re)
#define complex_imag_part(C) ((C).im)
#define complex_from_cart(RE,IM,R) \
  (((R)->re = (RE)), ((R)->im = (IM)))
#define complex_from_polar(A,P,R) \
  (((R)->re = (A)*cos(P)), ((R)->im = (A)*sin(P)))
#define complex_add(A,B,R) \
  (((R)->re = (A).re + (B).re), ((R)->im = (A).im + (B).im))
#define complex_mult(A,B,R) \
  (((R)->re = (A).re * (B).re - (A).im * (B).im), \
   ((R)->im = (A).re * (B).im + (A).im * (B).re))


/**************************************************************/
/* RAO model functions: */

#ifndef NO_READER

/* read the trk_sea_veh.rdr file, store the table into trk_sea_veh_ru,
 * and do some syntax checking of the table 
 */
STATIC int32 trk_sea_veh_ru_read(
    char  *data_path,
    uint32 flags);

/* Find the reader union describing a vehicle's sea variables.
 * Search for vehicle description which has a pair matching
 * field_name, field_value. 
 */
STATIC READER_UNION *trk_sea_get_veh_ru(
    char         *field_name,
    int           field_value_type, /* READER_INTEGER or READER_CHARPTR */
    READER_UNION  field_value);

/* Get a single field value. */
STATIC float64 trk_sea_get_veh_float(
    READER_UNION  *veh,
    char          *tagname);

/* Fill in the initial rao fields from the vehicle description, from
 * the reader file */
STATIC int32 tracked_rao_get_veh(
    READER_UNION      *veh,				  
    RAO_VEH           *rao);

#endif /*NO_READER*/

#ifndef NO_MODSAF

#endif /*not NO_MODSAF*/


/* Basic RAO functions */

#define TRK_SEA_FIELD_INVALID (-98765.)
#define SET_IF_NOT_SET(VAR, VALUE) \
    if (TRK_SEA_FIELD_INVALID == (VAR))  (VAR) = (VALUE)

/* Fill in invalid values, to start, so later we will know
 * which values have not yet been filled in. */
STATIC int32 tracked_rao_get_veh_invalid(RAO_VEH *rao);

/* void tracked_rao_generic_check_dims(RAO_VEH  *rao_veh)
 * Fill in any of mass,lens or waterline, which are currenly zero.*/
STATIC void  tracked_rao_generic_check_dims(RAO_VEH *rao_veh);

/* void  tracked_rao_generic_get_rao_poles(RAO_VEH  *rao_veh)
 * Fill in rao_poles (i.e. resonant frequency and damping) for
 * each RAO direction, using brick shape and known mass and lengths.*/
STATIC void  tracked_rao_generic_get_rao_poles(
    RAO_VEH  *rao_veh);

/* void  tracked_rao_generic_get_inertias(RAO_VEH  *rao_veh)
 * For each RAO dim, fill in mass and moments of inertia,
 * using given ship mass and lengths. */
STATIC void  tracked_rao_generic_get_inertias(
    RAO_VEH  *rao_veh);

/* void  tracked_rao_model(...)
 * Computes resulting position and absolute angle deflections, for each RAO 
 * direction, at the given time.  Requires all of the rao_veh struct
 * inputs (mass, lens, poles, inertias, pos, vel, y_direction)
 * to be filled in.  The input wave spectra will be used along with 
 * the ship's dimensions and position to compute the periodic forces 
 * on the ship (treating the ship like a brick).*/
STATIC void  tracked_rao_model(
    RAO_VEH  *rao_veh,
    float64  time,
    SPECTRA_ITER  *iter,
    int32    (*rao_spectra_iter)(SPECTRA_ITER      *iter,
				 float64            pos[3],
				 SEA_SPECTRAL_DATA_REC *spectra),
    float64  results[N_RAO_DIM]);

/* Used by tracked_rao_model() */

/* static void  tracked_rao_model_get_complex_force(...)
 * Returns a complex number which describes the periodic
 * force on the ship for the given RAO dimension and input wavelength.
 * The actual force can be obtained from the complex number result,
 * which has the correct amplitude and phase, by multiplying
 * F_complex by exp(-i*w*t), and taking the real part.
 */
STATIC void  tracked_rao_model_get_complex_force(
    RAO_DIM  d,
    float64  kp[2],
    float64  len[2],
    COMPLEX  *force);

/* void  tracked_rao_model_pole_response(...)
 * For a given pole (resonant frequency and damp factor), this
 * function computes the amplitude and phase response (as
 * a complex number) at the given driving frequency.  This factor
 * includes the ship dynamics portion of RAO, but not the
 * wave force portion of the RAO. */
STATIC void  tracked_rao_model_pole_response(
    RAO_POLE  *pole,
    float64    omega,
    COMPLEX   *response);

/**************************************************************/
/**************************************************************/

/* READER FILE STUFF */

/**************************************************************/


#ifndef NO_READER

static READER_UNION trk_sea_veh_ru;

/* read the trk_sea_veh_.rdr file, store the table into trk_sea_veh_ru,
 * and do some syntax checking of the table.
 */
STATIC int32 trk_sea_veh_ru_read(
    char  *data_path,
    uint32 flags)
{
    int32  rdr_error;
    char  *trk_sea_veh_filename = "trk_sea_veh.rdr";
    int32  i_len, i, j_len, j, k_len;
    READER_UNION  *veh, *field;


    /* Read from the trk_sea_veh.rdr file: */
    if (rdr_error = reader_read(trk_sea_veh_filename, data_path,
				&trk_sea_veh_ru, flags | READER_TYPING))
    {
	if (rdr_error == READER_READ_ERROR)
	  fprintf(stderr, "Syntax error in %s\n", trk_sea_veh_filename);
	else
	  fprintf(stderr, "Unable to open %s for defaults, aborting.\n", 
		  trk_sea_veh_filename);
	return(0);
    }
    /* check the table format, 
     * should be
     *   <table>      ::= ({<VEH>}*)
     *   <VEH>        ::= ({<field_pair>}*)
     *   <field_pair> ::= (<symbol> [<number> | <string>])
     */
    i_len = trk_sea_veh_ru.array[0].integer - 1;
    for (i=0; i < i_len; i++)
    {
	if ((READER_ARRAY != READER_UTYPE(trk_sea_veh_ru.array, i+1))
	    ||(0 > (j_len = (veh = trk_sea_veh_ru.array[i+1].array)[0].integer 
		                   - 1)))
	{
	    fprintf(stderr, "Bad table format in \"%s\"a, veh_entry %d\n", 
		    trk_sea_veh_filename, i);
	    return(0);
	}

	for (j=0; j < j_len; j++) 
	{
	    if ((READER_ARRAY != READER_UTYPE(veh, j+1))
                ||(3 != (k_len = (field = veh[j+1].array)[0].integer))
		||(READER_CHARPTR != READER_UTYPE(field, 1))
		||(!(  (READER_CHARPTR == READER_UTYPE(field, 2))
                     ||(READER_INTEGER == READER_UTYPE(field, 2))
                     ||(READER_REAL == READER_UTYPE(field, 2)))))
	    {
		fprintf(stderr, "Bad table format in \"%s\"a, veh_entry %d, field %d\n", 
			trk_sea_veh_filename, i, j);
		return(0);
	    }
	}
    }

    return(1);
}


/* Find the reader union describing a vehicle's sea variables.
 * Search for vehicle description which has a pair matching
 * field_name, field_value. 
 */
STATIC READER_UNION *trk_sea_get_veh_ru(
    char         *field_name,
    int           field_value_type, /* READER_INTEGER or READER_CHARPTR */
    READER_UNION  field_value)
{
    int           i_len, i;
    READER_UNION *veh, *field_pair;

    field_name = reader_get_symbol(field_name);
    i_len = trk_sea_veh_ru.array[0].integer - 1;
    for (i=0; i<i_len; i++)
    {
	veh = trk_sea_veh_ru.array[i+1].array;
	field_pair = reader_find_tag(field_name, veh,
				     READER_UNTAGGED, READER_NO_ERRORS);
	if (  (  (field_value_type == READER_INTEGER)
               &&(field_value.integer == field_pair[2].integer))
            ||(  (field_value_type == READER_CHARPTR)
	       &&(0==strcmp(field_value.charptr, field_pair[2].charptr))))
	  return(veh);
    }
    return((READER_UNION*)0);
}


/* Get a single field value. */
STATIC float64 trk_sea_get_veh_float(
    READER_UNION  *veh,
    char          *tagname)
{
    READER_UNION  *field_pair;

    if ((0!=(field_pair = reader_find_tag(reader_get_symbol(tagname), veh,
					  READER_UNTAGGED, READER_NO_ERRORS)))
	&&(field_pair[0].integer >= 3))
    {
	if (READER_UTYPE(field_pair, 2)==READER_INTEGER)
	  return((float64)(field_pair[2].integer));
	else if (READER_UTYPE(field_pair, 2)==READER_REAL)
	  return((float64)(field_pair[2].real));
    }
    return(TRK_SEA_FIELD_INVALID);
}



/* TRACKED_RAO SPECIFIC: */

/* Table for parsing reader file description of a vehicle into
 * the appropriate RAO_VEH fields.
 * This table will contain pairs: {reader_symbol_name, rao_vars_offset} 
 */
static struct veh_fields {
    char  *sym;
    int32  offset;
} tracked_rao_veh_fields[] = 
{
/* offsets will be stored as float64 array references */
#define AOFFSET(FIELD) \
  (((int32)(&((RAO_VEH*)0)->FIELD))/sizeof(float64))
   { "mass",                  AOFFSET(mass)},
   { "width",                 AOFFSET(len[0])},
   { "length",                AOFFSET(len[1])},
   { "height",                AOFFSET(len[2])},

   { "z_w0",                  AOFFSET(rao_poles[RAO_Z].w0)},
   { "z_damp",                AOFFSET(rao_poles[RAO_Z].damp)},
   { "pitch_w0",              AOFFSET(rao_poles[RAO_PITCH].w0)},
   { "pitch_damp",            AOFFSET(rao_poles[RAO_PITCH].damp)},
   { "roll_w0",               AOFFSET(rao_poles[RAO_ROLL].w0)},
   { "roll_damp",             AOFFSET(rao_poles[RAO_ROLL].damp)},

   { "z_inertia",             AOFFSET(inertias[RAO_Z])},
   { "pitch_inertia",         AOFFSET(inertias[RAO_PITCH])},
   { "roll_inertia",          AOFFSET(inertias[RAO_ROLL])},

   { "waterline",             AOFFSET(waterline)},
   { 0, 0 }
};

static int tracked_rao_veh_fields_inited = 0;

/* Fill in the initial rao fields from the vehicle description, from
 * the reader file */
STATIC int32 tracked_rao_get_veh(
    READER_UNION      *veh,				  
    RAO_VEH *rao)
{
    READER_UNION  *field;
    struct veh_fields *fieldsp;
    float64  *raoa;

    if (!tracked_rao_veh_fields_inited)
    {
	/* convert veh_fields strings into reader symbols */
	for (fieldsp = tracked_rao_veh_fields; fieldsp->sym; ++fieldsp)
	  fieldsp->sym = reader_get_symbol(fieldsp->sym);
	tracked_rao_veh_fields_inited = 1;
    }

    raoa = (float64*)(rao);
    for (fieldsp = tracked_rao_veh_fields; fieldsp->sym; ++fieldsp)
    {
	if ((field = reader_find_tag(fieldsp->sym, veh, READER_UNTAGGED,
				     READER_NO_ERRORS))
	    &&(field[0].integer >= 3))
	{
	    if (READER_UTYPE(field, 2)== READER_REAL)
            {
		SET_IF_NOT_SET(raoa[fieldsp->offset], field[2].real);
	    }
	    else if (READER_UTYPE(field, 2)== READER_INTEGER)
	    {
		SET_IF_NOT_SET(raoa[fieldsp->offset], field[2].integer);
	    }
	}
    }

    return(1);
}

#endif /*not NO_READER*/

/**************************************************************/
/**************************************************************/

/* MODSAF INTERFACE */

/**************************************************************/

#ifndef NO_MODSAF

/* local control vars: */
static int32    trk_rao_mode = 2; /* 0=non, 1=simple, 2=full */
static float64  trk_rao_time_delay_compensate = 0.; /* add to time for veh 
						     * calc. */
static float64  trk_rao_extra_z = 0.; /* shift vehicle up this extra amount */
static float64  trk_rao_response_attenuate = 0.9; /* fudge multiplier for responses*/
int32    tracked_rao_veh_id_debug = 0; /* id of vehicle to print debug info on */

/* local functions: */

/* Calculate the new roll, pitch and Z-offset of the specified entity,
 * based on the ocean heights. */
static void tracked_run_rao_simple(int32 vehicle_id, TRACKED_VARS *tracked,
				   ObjectType guise, float64 dt,
				   float32 *roll, float32 *pitch, 
				   float64 posit[3], float64 dir[3]);

/* Calculate the new roll, pitch and Z-offset of the specified entity,
 * based on its RAOs. */
static void tracked_run_rao_full(int32 vehicle_id, TRACKED_VARS *tracked,
                                 SPECTRA_ITER  *iter,
				 ObjectType guise, float64 dt,
				 float32 *roll, float32 *pitch, 
				 float64 posit[3], float64 dir[3], 
				 float64 vel[3]);

static int32 tracked_rao_get_veh_physdb_dims(
    int32     vehicle_id,
    RAO_VEH  *rao_veh);

/* to be used in a loop looking like:
 * for (tracked_rao_wave_spectra_iter(iter,pos,0);
 *      tracked_rao_wave_spectra_iter(iter,pos,&spectra); )
 *   ...
 */
static int32  tracked_rao_wave_spectra_iter(
    SPECTRA_ITER      *iter,
    float64            pos[3],
    SEA_SPECTRAL_DATA_REC *spectra);

#ifdef OLD_FLAT
static void process_reader_file(char *data_path, char *filename);
#endif

/**************************************************************/

/* ModSAF interface */

#ifdef USE_PARSER
#include <stdio.h>
#include <libparser.h>
#include <libenvui.h>

static void trk_rao_parser_init(void);
#endif

void tracked_init_rao(char *data_path)
{
    /*if (!trk_sea_veh_ru_read(data_path, READER_DEFAULTS))  return(0);*/
    trk_sea_veh_ru_read(data_path, READER_DEFAULTS);
#ifdef USE_PARSER
    trk_rao_parser_init();
#else
    ;
#endif
}

int32 tracked_have_rao(ObjectType guise)
{
#ifndef OLD
    READER_UNION  guise_ru, *veh_ru;
    float64       val;

    guise_ru.integer = guise;
    if (0!=(veh_ru = trk_sea_get_veh_ru("guise", READER_INTEGER, guise_ru)))
    {
	val = trk_sea_get_veh_float(veh_ru, "no_wave_response");
	if ((val==0)||(val==TRK_SEA_FIELD_INVALID))  return(TRUE);
    }
    return(FALSE);
#else
    switch (guise)
    {
      case vehicle_USMC_AAV:
      case vehicle_USMC_AAV_C7:
      case vehicle_USMC_AAV_MINE:
      case vehicle_USMC_AAV_REC:
	return TRUE;
      case vehicle_US_DD963:
	return TRUE;
      default:
	return FALSE;
    }
#endif
}

/* Computes new absolute angles and z position,
 * using time synchronized to the wave.  The full model
 * uses a steady state model, and the correct x y position
 * to find the absolute z position and angle deviations. */
void tracked_run_rao(int32 vehicle_id, TRACKED_VARS *tracked,
		     ObjectType guise, float64 dt,
		     float32 *roll, float32 *pitch, 
		     float64 posit[4], float64 dir[3], float64 vel[3])
{
    SPECTRA_ITER  iter;
    float32       pos32[4], vel32[4], out32[4];
    float64       lposit[3], ldir[3], lvel[3]; /* in EnvXYZ coordinates */
    ecloud_envxyz *envxyz = 0;
    CTDB    *ctdb = gcs_get_tdb((int32)posit[CELL3D], FALSE);
    CTDB_ELEV_DATA           elev[CTDB_MAX_ELEVS];
    float64 water_z;
    int32 eidx;

    /* convert positions etc. to EnvXYZ coords */
    VMAT4_RECAST_VEC(posit, pos32);
    ecloud_envxyz_gcs_to_envxyz32(envxyz, pos32, out32, NULL);
    VMAT3_RECAST_VEC(out32, lposit);

    VMAT3_RECAST_VEC(vel, vel32);
    vel32[CELL3D] = pos32[CELL3D];
    ecloud_envxyz_gcs_to_envxyz32(envxyz, vel32, out32, pos32);
    VMAT3_RECAST_VEC(out32, lvel);

    VMAT3_RECAST_VEC(dir, vel32);
    vel32[CELL3D] = pos32[CELL3D];
    ecloud_envxyz_gcs_to_envxyz32(envxyz, vel32, out32, pos32);
    VMAT3_RECAST_VEC(out32, ldir);

    VMAT4_RECAST_VEC(posit, iter.pos0); /* save GCS position */
    /* do model */
    if (trk_rao_mode==1)
      tracked_run_rao_simple(vehicle_id, tracked,
			     guise, dt, roll, pitch, posit, dir);
    else if (trk_rao_mode==2)
      tracked_run_rao_full(vehicle_id, tracked, &iter,

			   guise, dt, roll, pitch, posit, dir, vel);
    else
    {
	*roll = 0;  *pitch = 0;	posit[2] = 0;
    }

    /* Find the Z-value of the water surface.  With the transition to
     * multi-cell GCS databases, this is not usually (if ever) zero,
     * and is typically negative, yielding "flying AAV's".
     *
     */
    ctdb_lookup_elevation_mes(ctdb, posit[X], posit[Y],
			      CTDB_LAND_AND_WATER, elev);
    water_z = 0.0;
    for (eidx = 0; eidx < CTDB_MAX_ELEVS; ++eidx)
    {

	if (elev[eidx].tdbclass == CTDB_FC_ILLEGAL) /* denotes end */
	  break;
	
	if (elev[eidx].tdbclass == CTDB_FC_MICRO)
	    if (elev[eidx].subclass == CTDB_FS_WATER)
		water_z = elev[eidx].z;
    }

    posit[2] += trk_rao_extra_z;

    posit[Z] += water_z; /* Adjust for elevation of water surface. */
    
    if (vehicle_id == tracked_rao_veh_id_debug)
	printf("veh %d: t= %lg, z_off= %lg, pitch= %g, roll= %g\n",
	       vehicle_id, dt, posit[2], *pitch, *roll);
}

/**************************************************************/

#ifdef  USE_PARSER

static void trk_rao_mode_set(
    int32 argc,
    int32 argv[])
{
    trk_rao_mode = argv[0];
}

static void trk_rao_time_delay_compensate_set(
    int32 argc,
    float32 argv[])
{
    trk_rao_time_delay_compensate = argv[0];
}

static void trk_rao_extra_z_set(
    int32 argc,
    float32  argv[])
{
    trk_rao_extra_z = argv[0];
}

static void trk_rao_response_attenuate_set(
    int32 argc,
    float32  argv[])
{
    trk_rao_response_attenuate = argv[0];
}

static void trk_rao_veh_id_debug_set(
    int32 argc,
    int32 argv[])
{
    tracked_rao_veh_id_debug = argv[0];
}

static void trk_rao_vars_print(
    int32 argc,
    float32  argv[])
{
    static  char *modes[3] = {"non", "simple", "full"};

    printf("                 trk_rao_mode = %s\n",
	   modes[ ((0 <= trk_rao_mode) && (trk_rao_mode < 3))
		 ?(trk_rao_mode):(0)]);
    printf("trk_rao_time_delay_compensate = %lg\n",
	   trk_rao_time_delay_compensate);
    printf("              trk_rao_extra_z = %lg\n", trk_rao_extra_z);
    printf("   trk_rao_response_attenuate = %lg\n",
	   trk_rao_response_attenuate);
    printf("         trk_rao_veh_id_debug = %ld\n",
	   tracked_rao_veh_id_debug);
}

#ifndef lint

/* libparser command tables, for the command being added the the
 * text user interface. */

static CONSTANT_TABLE(trk_rao_modes_table)
    CONSTANT("non",    0, "- do no RAO calculations")
    CONSTANT("simple", 1, "- get orientation from sea heights")
    CONSTANT("full",   2, "- full RAO calculations")
END_CONSTANT_TABLE

static DEFINE_TABLE(trk_rao_local_command_table)
  KEYWORD_SELECT("     tracked ship RAO commands (libtracked) ")
    KEYWORD ("print", "- print RAO calculation mode etc")
      CALL (trk_rao_vars_print)
    END_KEYWORD
    KEYWORD ("mode", "- RAO calculation mode: non or simple or full") 
      GETCONSTANT(trk_rao_modes_table)
      CALL (trk_rao_mode_set)
    END_KEYWORD
    KEYWORD ("time_delay_compensate", "- produce ship positions ahead of time")
      GETFLOAT("time ahead (seconds)")
      CALL (trk_rao_time_delay_compensate_set)
    END_KEYWORD
    KEYWORD ("extra_z", "- extra ship elevation, for every ship")
      GETFLOAT("z")
      CALL (trk_rao_extra_z_set)
    END_KEYWORD
    KEYWORD ("attenuate_response", "- extra ship response attenuation (default 1.0)")
      GETFLOAT("attenuation_factor")
      CALL (trk_rao_response_attenuate_set)
    END_KEYWORD
    KEYWORD ("veh_id_debug", "- vehicle id for printing debug info on")
      GETDECIMAL("vehicle_id")
      CALL (trk_rao_veh_id_debug_set)
    END_KEYWORD
  END_KEYWORD_SELECT
END_DEFINE_TABLE

static DEFINE_TABLE(trk_rao_command_table)
    KEYWORD ("trk_rao", "- tracked ship RAO commands (libtracked)")
      DO_KEYWORD_TABLE(trk_rao_local_command_table)
    END_KEYWORD
END_DEFINE_TABLE

#endif /* lint */

static void trk_rao_parser_init(void)
{
    ParseAddTable(eui_command_table, trk_rao_command_table);
}

#endif /* USE_PARSER */

/**************************************************************/
/* locked to ocean RAO model */

/*
 * read in the tracked data file... 
 * the rdr file should be in this format:
 * 
 *   ( 
 *     (max_speed
 *         (AVLB 4.0)
 *         (on_bridge 4.0)
 *         (in_breach_lane 4.0)
 *     )
 *   )
 *
 */

#if 0
static void process_reader_file(char *data_path, char *filename)
    READER_UNION ru, *field, *speeds;

    if (reader_read(filename, data_path, &ru, READER_DEFAULTS) != 
	    READER_READ_OK)
    {
        fprintf(stderr, "\nCouldn't find %s..  using defaults\n", filename);
        return;
    }

    if ((tracked_speeds = reader_find_tag(reader_get_symbol("max_speed"), 
				  ru.array, READER_UNTAGGED, 0)) == NULL)
    {
        fprintf(stderr, "\nProblem reading %s..\n", filename);
        return;
    }
}
#endif


/*
 * Calculate the new roll, pitch and Z-offset of the specified entity,
 * based on the ocean heights. 
 */
static void tracked_run_rao_simple(int32 vehicle_id, TRACKED_VARS *tracked,
				   ObjectType guise, float64 dt,
				   float32 *roll, float32 *pitch, 
				   float64 posit[3], float64 dir[3])
{

    float32 alt_roll, alt_pitch;
    ENV_INTERFACE ei;        /* environmental interface */
    uint32 num_waves;
    uint32 num_frequencies;
    float64 tide_level, my_length, my_beam, new_z /*,dir[3]*/;
    float64 ctr_height, bow_height, stern_height, port_height, stbd_height;
    int32 wavei, freqi;
    static float32 pitch_limit = 30.0, roll_limit = 30.0;

    new_z = posit[Z];
    /*
     * $$$ Hard-coded Z adjustment: Spruance has draft of 29 feet.
     *                              AAV has draft (eyeballed) of 4 feet.
     */
    switch (guise)
    {
      case vehicle_USMC_AAV:
      case vehicle_USMC_AAV_C7:
      case vehicle_USMC_AAV_MINE:
      case vehicle_USMC_AAV_REC:
#if 0
	new_z -= FT_TO_M(3.9);
#endif
	my_length = 8.2;
	my_beam = 3.3;
	break;
      case vehicle_US_DD963:
      default:
#if 0
	new_z -= FT_TO_M(29.0)
#endif
	my_length = FT_TO_M(563.0);
	my_beam = FT_TO_M(55.0);
	break;
#if 0
      default:
	return;
#endif
    }

#define FAKE_OUT_LIBENVSEA_TIME
#ifdef FAKE_OUT_LIBENVSEA_TIME
    /* temporarily change envsea_base_time for env_gets on sea height */
    envsea_base_time -= trk_rao_time_delay_compensate;
#endif

    /*
     * Get the sea height at the bow, stern, starboard and port edges.
     * Hard-coded offsets represent the dimensions of an AAV.  This
     * has the side effect of exercising the underlying P-M model,
     * which has an expense we'd like to assess.
     */
    bzero(&ei, sizeof(ENV_INTERFACE));    
    ei.u.sea_surface_height.input_vector[X] = posit[X];
    ei.u.sea_surface_height.input_vector[Y] = posit[Y];
    ei.u.sea_surface_height.input_vector[Z] = posit[Z];
    ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D];
    env_get(ENV_SEA_SURFACE_HEIGHT, &ei);
    ctr_height = ei.u.sea_surface_height.output_value;
    new_z += ctr_height;

    /*ent_get_direction(vehicle_id, dir);*/

    bzero(&ei, sizeof(ENV_INTERFACE));    
    ei.u.sea_surface_height.input_vector[X] = 
      posit[X] + dir[1] * (my_length/2.0);
    ei.u.sea_surface_height.input_vector[Y] = 
      posit[Y] + dir[0] * (my_length / 2.0);
    ei.u.sea_surface_height.input_vector[Z] = posit[Z];
    env_get(ENV_SEA_SURFACE_HEIGHT, &ei);
    bow_height = ei.u.sea_surface_height.output_value;

    bzero(&ei, sizeof(ENV_INTERFACE));    
    ei.u.sea_surface_height.input_vector[X] = 
      posit[X] - dir[1] * (my_length/2.0);
    ei.u.sea_surface_height.input_vector[Y] = 
      posit[Y] - dir[0] * (my_length / 2.0);
    ei.u.sea_surface_height.input_vector[Z] = posit[Z];
    ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D];
    env_get(ENV_SEA_SURFACE_HEIGHT, &ei);
    stern_height = ei.u.sea_surface_height.output_value;

    bzero(&ei, sizeof(ENV_INTERFACE));    
    ei.u.sea_surface_height.input_vector[X] = 
      posit[X] + dir[0] * (my_beam / 2.0);
    ei.u.sea_surface_height.input_vector[Y] =
      posit[Y] + dir[1] * (my_beam / 2.0);
    ei.u.sea_surface_height.input_vector[Z] = posit[Z];
    ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D];
    env_get(ENV_SEA_SURFACE_HEIGHT, &ei);
    stbd_height = ei.u.sea_surface_height.output_value;

    bzero(&ei, sizeof(ENV_INTERFACE));    
    ei.u.sea_surface_height.input_vector[X] = 
      posit[X] - dir[0] * (my_beam / 2.0);
    ei.u.sea_surface_height.input_vector[Y] =
      posit[Y] - dir[1] * (my_beam / 2.0);
    ei.u.sea_surface_height.input_vector[Z] = posit[Z];
    ei.u.sea_surface_height.input_vector[CELL3D] = posit[CELL3D];
    env_get(ENV_SEA_SURFACE_HEIGHT, &ei);
    port_height = ei.u.sea_surface_height.output_value;

#ifdef FAKE_OUT_LIBENVSEA_TIME
    /* put back */
    envsea_base_time += trk_rao_time_delay_compensate;
#endif

    alt_roll  = asin((port_height-stbd_height)/my_beam);
    alt_pitch = asin((stern_height-bow_height)/my_length);
    
    /*
     * Protect ourselves from impossible/inconvenient roll/pitch
     * values by clipping to +/- 30 degrees.
     */
    if (alt_roll > DEG_TO_RAD(roll_limit))
    {
#if 0
	DEBUG_TRACKED("Roll clipped from %.1f\n", RAD_TO_DEG(alt_roll));
#endif
	alt_roll = DEG_TO_RAD(roll_limit);
    }
    else if (alt_roll < DEG_TO_RAD(-roll_limit))
    {
#if 0
	DEBUG_TRACKED("Roll clipped from %.1f\n", RAD_TO_DEG(alt_roll));
#endif
	alt_roll = DEG_TO_RAD(-roll_limit);
    }
    if (alt_pitch > DEG_TO_RAD(pitch_limit))
    {
#if 0
	DEBUG_TRACKED("Pitch clipped from %.1f\n", RAD_TO_DEG(alt_pitch));
#endif
	alt_pitch = DEG_TO_RAD(pitch_limit);
    }
    else if (alt_pitch < DEG_TO_RAD(-pitch_limit))
    {
#if 0
	DEBUG_TRACKED("Pitch clipped from %.1f\n", RAD_TO_DEG(alt_pitch));
#endif
	alt_pitch = DEG_TO_RAD(-pitch_limit);
    }

    *roll = alt_roll;
    *pitch = alt_pitch;

    posit[Z] = new_z;

}


/**************************************************************/
/* full RAO model. */

#define CHECK_ANGLE_LIMIT(VAR,LIM) \
  if (fabs((VAR))>(LIM)) (VAR)=(LIM)*(((VAR)<0)?(-1.):(1))
#define TRACKED_RAO_ANGLE_LIMIT  ((40./180.)*PI)

/* Calculate the new roll, pitch and Z-offset of the specified entity,
 * based on its RAOs. */
static void tracked_run_rao_full(int32 vehicle_id, TRACKED_VARS *tracked,
                                 SPECTRA_ITER  *iter,
				 ObjectType guise, float64 dt,
				 float32 *roll, float32 *pitch, 
				 float64 posit[3], float64 dir[3],
				 float64 vel[3])
{
    RAO_VEH          rao_veh;  /* the struct which is incrementally filled in
                                * for this RAO model. */
    float64          results[N_RAO_DIM];
    ENV_INTERFACE    ei;
    float64          tide_level;
    struct timeval   time_ptr;
    READER_UNION     guise_ru, *veh_ru;

    /* get tide_level */
    bzero(&ei, sizeof(ei.u.sea_tide_level.input_vector));
    bcopy(posit, &ei.u.sea_tide_level.input_vector, 3*sizeof(float64));
    tide_level = 0.;
    if (env_get(ENV_SEA_TIDE_LEVEL, &ei) != -1)
      tide_level += ei.u.sea_tide_level.output_value;

#define TRK_RAO_SEA_TIME
#ifdef TRK_RAO_SEA_TIME
    gettimeofday(&time_ptr, (struct timezone *)NULL);
    dt = (time_ptr.tv_sec + time_ptr.tv_usec * .000001)-
      envsea_base_time;
    dt += trk_rao_time_delay_compensate;
#endif


    /* fill rao_veh */
    memset(&rao_veh, 0, sizeof(RAO_VEH));
    /* first fill with invalid values so that the first function to set 
     * a field is the only function to set that field */
    tracked_rao_get_veh_invalid(&rao_veh);
    guise_ru.integer = ent_get_guise(vehicle_id, 0);
    if (0!=(veh_ru = trk_sea_get_veh_ru("guise", READER_INTEGER, guise_ru)))
      tracked_rao_get_veh(veh_ru, &rao_veh);  /*from reader file entry*/
    tracked_rao_get_veh_physdb_dims(vehicle_id, &rao_veh); /* from physdb */
    /* compute any entries not already filled in: */
    tracked_rao_generic_check_dims(&rao_veh);
    tracked_rao_generic_get_rao_poles(&rao_veh);
    tracked_rao_generic_get_inertias(&rao_veh);


    /* copy pos, vel */
    memcpy(&rao_veh.pos, posit, 3*sizeof(float64));
    memcpy(&rao_veh.vel, vel, 3*sizeof(float64));
    vmat2_unit64(dir, rao_veh.y_direction);

    /* do the real calculation: */
    tracked_rao_model(&rao_veh, dt, iter,
		      tracked_rao_wave_spectra_iter,
		      results);
    posit[2] = results[RAO_Z] /*height deviation*/
               * trk_rao_response_attenuate
               + tide_level
               - rao_veh.waterline; /* bottom height minus 
				     * zero height sea level */
    CHECK_ANGLE_LIMIT(results[RAO_PITCH], TRACKED_RAO_ANGLE_LIMIT);
    CHECK_ANGLE_LIMIT(results[RAO_ROLL], TRACKED_RAO_ANGLE_LIMIT);
    *pitch   = results[RAO_PITCH] * trk_rao_response_attenuate;
    *roll    = results[RAO_ROLL]  * trk_rao_response_attenuate;
/*#define TRK_DEBUG2*/
#ifdef TRK_DEBUG2
    printf("veh %d: t= %lg, z_off= %lg, pitch= %g, roll= %g\n",
	   vehicle_id, dt, posit[2], *pitch, *roll);
#endif
}

#include <libphysdb.h>

static int32 tracked_rao_get_veh_physdb_dims(
    int32     vehicle_id,
    RAO_VEH  *rao_veh)
{
    PHYSDB_DATA *pdb;
    int32       i;

    /* first get data from physdb: */
    pdb = ent_get_physdb(vehicle_id);
    SET_IF_NOT_SET(rao_veh->mass, pdb->mass_kg);
    for (i=0; i<3; i++)  
      SET_IF_NOT_SET(rao_veh->len[i], pdb->dimensions[i]);
    SET_IF_NOT_SET(rao_veh->waterline, pdb->model_base_adjustment);

    return(1);
}


/* to be used in a loop looking like:
 * for (tracked_rao_wave_spectra_iter(iter,pos,0);
 *      tracked_rao_wave_spectra_iter(iter,pos,&spectra); )
 *   ...
 */
static int32  tracked_rao_wave_spectra_iter(
    SPECTRA_ITER      *iter,
    float64            pos[3],
    SEA_SPECTRAL_DATA_REC *spectra)
{
    ENV_INTERFACE ei;        /* environmental interface */

    if (!spectra)
    {
	bzero(&ei, sizeof(ENV_INTERFACE));    
	env_get(ENV_SEA_NUM_WAVES, &ei);
	iter->nwaves = ei.u.sea_num_waves.output_type;

	bzero(&ei, sizeof(ENV_INTERFACE));    
	env_get(ENV_SEA_NUM_FREQUENCIES, &ei);
	iter->nfreqs = ei.u.sea_num_frequencies.output_type;

	iter->iwave = -1;
	iter->ifreq = 0;
	return(1);
    }
    else
    {
	if (  (iter->ifreq >= iter->nfreqs)  
            ||(  (++(iter->iwave) >= iter->nwaves)
	       &&(++(iter->ifreq) >= iter->nfreqs)))
	  return(0);
	if (iter->iwave >= iter->nwaves)  iter->iwave = 0;

	bzero(&ei, sizeof(ENV_INTERFACE));    
	ei.u.spectral_data.long_wave_num = iter->iwave;
	ei.u.spectral_data.frequency_num = iter->ifreq;
	VMAT4_RECAST_VEC(iter->pos0, ei.u.spectral_data.input_vector);
	env_get(ENV_SEA_SPECTRAL_RECORD, &ei);
	spectra->frequency      =  ei.u.spectral_data.output_frequency;
	spectra->amplitude      =  ei.u.spectral_data.output_amplitude;
	spectra->phase          =  ei.u.spectral_data.output_phase;
	spectra->wave_number[0] =  ei.u.spectral_data.output_wave_number_X;
	spectra->wave_number[1] =  ei.u.spectral_data.output_wave_number_Y;
	
	return(1);
    }
}

/**************************************************************/

#endif /*not NO_MODSAF*/

/**************************************************************/
/**************************************************************/

/* THIS RAO MODEL: */

/**************************************************************/


/* Fill in invalid values, to start, so later we will know
 * which values have not yet been filled in. */
STATIC int32 tracked_rao_get_veh_invalid(RAO_VEH *rao)
{
    float64  *raoa, *raob;

    raob = (float64*)(&(rao->pos[0]));
    for (raoa = (float64*)(&(rao->mass)); raoa < raob; ++raoa)
      *raoa = TRK_SEA_FIELD_INVALID;
    return(1);
}

/* void tracked_rao_generic_check_dims(RAO_VEH  *rao_veh)
 * Fill in any of mass,lens or waterline, which are currenly zero.*/
STATIC void  tracked_rao_generic_check_dims(
    RAO_VEH  *rao_veh)
{
    int32       i;
    float64     temp;
    
    /* force to be nonzeros: */
    if (rao_veh->mass <= 0)  rao_veh->mass = 10000.;
    for (i=0; i<3; i++)  
      if (rao_veh->len[i] <= 0)  rao_veh->len[i] = 10.;

    if (rao_veh->waterline <= 0)
    {
	/* approximate depth in water, treating ship as a uniform 
	 * density box: */
	temp = WATER_DENSITY /* kg/m^3 */ 
	       * rao_veh->len[0] * rao_veh->len[1];
	if (temp > 0.)  rao_veh->waterline = rao_veh->mass / temp;
	else            rao_veh->waterline = 0;
    }
}

/* void  tracked_rao_generic_get_rao_poles(RAO_VEH  *rao_veh)
 * Fill in rao_poles (i.e. resonant frequency and damping) for
 * each RAO direction, using brick shape and known mass and lengths.*/
/* These pole formulae describe the ship dynamics porition of the RAO,
 * and were derived by treating the ship as a brick, and deriving
 * the restoring forces on this brick given small position or angle
 * deviations.  The restoring forces were calculated using a local verion
 * of the Archemedies displaced water principle described below. */
STATIC void  tracked_rao_generic_get_rao_poles(
    RAO_VEH  *rao_veh)
{
    RAO_DIM  d;
    float64  *len;

    if (  (rao_veh->mass == 0)
	||(rao_veh->len[0] == 0)
	||(rao_veh->len[1] == 0)
	||(rao_veh->len[2] == 0))
    {
	for (d=0; d<N_RAO_DIM; d++)
	  SET_IF_NOT_SET(rao_veh->rao_poles[d].w0, 1);
    }
    else
    {
	/* simple formulae */
	len = rao_veh->len;
	SET_IF_NOT_SET(rao_veh->rao_poles[RAO_Z].w0,
		       sqrt( GRAVITY * WATER_DENSITY * len[0] * len[1]
			    / rao_veh->mass ));
	SET_IF_NOT_SET(rao_veh->rao_poles[RAO_PITCH].w0, /* about X axis */
		       rao_veh->rao_poles[RAO_Z].w0
		       / sqrt( 1. + ( (len[2] * len[2]) 
				     / (len[1] * len[1]) ) ) );
	SET_IF_NOT_SET(rao_veh->rao_poles[RAO_ROLL].w0,  /* about Y axis */
		       rao_veh->rao_poles[RAO_Z].w0
		       / sqrt( 1. + ( (len[2] * len[2]) 
				     / (len[0] * len[0]) ) ) );
    }
    /* fudge damp factors for now: */
    for (d=0; d<N_RAO_DIM; d++)
      SET_IF_NOT_SET(rao_veh->rao_poles[d].damp, DEFAULT_RAO_DAMP_CONSTANT);
}

/* void  tracked_rao_generic_get_inertias(RAO_VEH  *rao_veh)
 * For each RAO dim, fill in mass and moments of inertia,
 * using given ship mass and lengths. */
STATIC void  tracked_rao_generic_get_inertias(
    RAO_VEH  *rao_veh)
{
    RAO_DIM  d;
    float64  *len;

    if (  (rao_veh->mass == 0)
	||(rao_veh->len[0] == 0)
	||(rao_veh->len[1] == 0)
	||(rao_veh->len[2] == 0))
    {
	for (d=0; d<N_RAO_DIM; d++)
	  SET_IF_NOT_SET(rao_veh->inertias[d], 1.);
    }
    else
    {
	/* simple formulae */
	len = rao_veh->len;
	SET_IF_NOT_SET(rao_veh->inertias[RAO_Z],
		       rao_veh->mass);
	SET_IF_NOT_SET(rao_veh->inertias[RAO_PITCH],
		       rao_veh->mass * (len[1]*len[1] + len[2]*len[2]) / 12.);
	SET_IF_NOT_SET(rao_veh->inertias[RAO_ROLL],
		       rao_veh->mass * (len[0]*len[0] + len[2]*len[2]) / 12.);
    }
}

/* void  tracked_rao_model(...)
 * Computes resulting position and absolute angle deflections, for each RAO 
 * direction, at the given time.  Requires all of the rao_veh struct
 * inputs (mass, lens, poles, inertias, pos, vel, y_direction)
 * to be filled in.  The input wave spectra will be used along with 
 * the ship's dimensions and position to compute the periodic forces 
 * on the ship (treating the ship like a brick). */
STATIC void  tracked_rao_model(
    RAO_VEH  *rao_veh,
    float64  time,
    SPECTRA_ITER  *iter,
    int32    (*rao_spectra_iter)(SPECTRA_ITER      *iter,
				 float64            pos[3],
				 SEA_SPECTRAL_DATA_REC *spectra),
    float64  results[N_RAO_DIM])
{
    SEA_SPECTRAL_DATA_REC  spectra;
    RAO_DIM            d;
    float64            frequency, k[2], kp[2]; /* wave vector in ship frame */
    COMPLEX            forces[N_RAO_DIM], phase, response[3];
#define DOT2(V1,V2) (((V1)[0]*((V2)[0])) + ((V1)[1]*((V2)[1])))
    
    
    /* for each rao dim, clear result */
    for (d=0; d<N_RAO_DIM; d++)
      results[d] = 0;
      
    /* for each wave component, find each rao response */
    for ((*rao_spectra_iter)(iter, rao_veh->pos, 0);
	 (*rao_spectra_iter)(iter, rao_veh->pos, &spectra); )
    {
	/* compensate for backwards wave vector in PM model: */
        k[0] = spectra.wave_number[0];
        k[1] = spectra.wave_number[1];
	/* phase shift from center of ship position etc. */
	complex_from_polar(1.,
			   /* uses normal physics convention k.r - w*t: */
			   + DOT2(k, rao_veh->pos)
			   - spectra.frequency * time
			   + spectra.phase, /* pm has backwards time */
			   &phase);

	/* frequency shift from ship velocity */
	frequency = spectra.frequency
	            - DOT2(k, rao_veh->vel);

	/* wave_vector in ship frame */
	kp[1] = DOT2(k, rao_veh->y_direction);
	kp[0] =  k[0] * rao_veh->y_direction[1]
	       - k[1] * rao_veh->y_direction[0];

        /* for each rao_dim, add in the frequency component response */
	for (d=0; d<N_RAO_DIM; d++)
	{
	    tracked_rao_model_get_complex_force(d, kp, rao_veh->len, 
						&forces[d]);
				      
	    tracked_rao_model_pole_response(&(rao_veh->rao_poles[d]), 
					    frequency, 
					    &response[0]);
	    complex_mult(phase, forces[d], &response[1]);
	    complex_mult(response[1], response[0], &response[2]);

	    results[d] +=
	        (spectra.amplitude/rao_veh->inertias[d])
		* complex_real_part(response[2]);
	}
    }
}


/* static void  tracked_rao_model_get_complex_force(...)
 * Returns a complex number which describes the periodic
 * force on the ship for the given RAO dimension and input wavelength.
 * The actual force can be obtained from the complex number result,
 * which has the correct amplitude and phase, by multiplying
 * F_complex by exp(-i*w*t), and taking the real part.
 */
/* This function uses the Archmedies principle locally at each
 * point on the under surface of the ship to find the force there:
 * this local interpretation of Archemedies principle states that
 * the upwards force on the ship is equal to the weight of the
 * column of displaced water above that point on the ship (including
 * wave height).  For the purposes of integrating this force,
 * the ship is treated as a rectangular brick, in a horizontal position.
 * The effective restoring force was already included when the pole
 * frequencies were calculated.
 */
STATIC void  tracked_rao_model_get_complex_force(
    RAO_DIM  d,
    float64  kp[2],
    float64  len[2],
    COMPLEX  *force)
{
    int32    moment[2], i;
    float64  kX[2], result;

    moment[0]=0;  moment[1]=0;  result = 1.;
    if (d == RAO_PITCH)  moment[1] = 1;
    if (d == RAO_ROLL)   { moment[0] = 1;  result *=-1; }
    for (i=0; i<2; i++)
    {
#define EPS1  1.e-3
	kX[i] = 0.5*kp[i]*len[i];
	if (moment[i]==0)
	{
	    if (fabs(kX[i]) > EPS1)  
	      result *= (2./kp[i])*sin(kX[i]);
	    else                    
	      result *= len[i];
	}
	else
	{
	    if (fabs(kX[i]) > EPS1)  
	      result *= (-(len[i]/kp[i])*cos(kX[i])
			 +(2./(kp[i]*kp[i]))*sin(kX[i]));
	    else
	      result *= - kX[i]*len[i]*len[i]/6.;
	}
    }
    result *= GRAVITY * WATER_DENSITY;
    if (moment[0] || moment[1])
      complex_from_cart(0., result, force);
    else
      complex_from_cart(result, 0., force);
}


/* void  tracked_rao_model_pole_response(...)
 * For a given pole (resonant frequency and damp factor), this
 * function computes the amplitude and phase response (as
 * a complex number) at the given driving frequency.  This factor
 * includes the ship dynamics portion of RAO, but not the
 * wave force portion of the RAO. */
STATIC void  tracked_rao_model_pole_response(
    RAO_POLE  *pole,
    float64    omega,
    COMPLEX   *response)
{
    /* damp is very roughly the decay fraction per cycle,
     * friction coeff b = damp*w0/Pi, this properly defines damp
     * pole_response(w) = 1/((-I*w)**2 + (-I*w)*b + w0**2)
     *                  = (w0**2 - w**2 + I*w*b)
     *                    /((w0**2 - w**2)**2 + (w*b)**2).
     */
    float64  bw, w02minusw2, mag2;

    bw = pole->damp * omega * (1./PI);
    w02minusw2 = ( pole->w0 * pole->w0 ) - ( omega * omega );
    if (0 != (mag2 = w02minusw2*w02minusw2 + bw*bw))
      complex_from_cart(w02minusw2/mag2, bw/mag2, response);
    else
      complex_from_cart(0., 0., response);
}

/**************************************************************/
/**************************************************************/

/* RAO MODEL TEST PROGRAM: */

/**************************************************************/

#ifdef TRACKED_RAO_TEST

/* test program for testing this RAO model */

#include <stdio.h>

static int32  tracked_rao_test_spectra_iter(
    SPECTRA_ITER      *iter,
    float64            pos[3],
    SEA_SPECTRAL_DATA_REC *spectra);

SEA_SPECTRAL_DATA_REC  test_spectra = {0, 1, 0, {0, 1}};

int32 usage(int32 iarg, char *argv[])
{
    if (iarg > 0)
      fprintf(stderr,"ERROR: bad argument to raotest %d = \"%s\" \n",
	      iarg, argv[iarg]);
#ifndef NO_READER
    printf("Usage: test_trk_rao vehicle_name \\\n");
#else
    printf("Usage: testrao shipmass(kg) width(m) length(m) height(m) \\\n");
    printf("          dampz damppitch damproll \\\n");
#endif
    printf("          waveheadingfromship(degrees) wave_omega\n");
#ifdef NO_READER
    printf(" damp factors are roughly fraction per resonance cycle\n");
    printf(" e.g. 0.07 to 0.5 \n");
#endif
    printf(" wave heading is a compass direction from north pointing ship\n");
    return(0);
}


int32 main(int32 argc, char *argv[])
{
    int32    iarg;
    RAO_VEH  rao_veh;
#ifndef NO_READER
    READER_UNION  veh_name_ru, *veh;
#endif
    float64  damp[N_RAO_DIM], waveheadingfromship;
    float64  wave_number, angle;
    RAO_DIM  d;
    float64  results_cos[N_RAO_DIM], results_sin[N_RAO_DIM];
    float64  results_amp[N_RAO_DIM], results_phase[N_RAO_DIM];
    float64  results_amp2[N_RAO_DIM];
    static char *rao_dim_names[N_RAO_DIM] = 
             {"Z    ","PITCH","ROLL "};

    memset(&rao_veh, 0, sizeof(RAO_VEH));
    tracked_rao_get_veh_invalid(&rao_veh);

    iarg = 0;
#ifndef NO_READER
    if (  (argc < 4)
        ||(!(veh_name_ru.charptr = argv[++iarg]))
#else
    if (  (argc < 10)
	||(1>sscanf(argv[++iarg],"%lf", &rao_veh.mass)) 
	||(1>sscanf(argv[++iarg],"%lf", &rao_veh.len[0])) 
	||(1>sscanf(argv[++iarg],"%lf", &rao_veh.len[1])) 
	||(1>sscanf(argv[++iarg],"%lf", &rao_veh.len[2])) 
	||(1>sscanf(argv[++iarg],"%lf", &damp[RAO_Z]))
	||(1>sscanf(argv[++iarg],"%lf", &damp[RAO_PITCH]))
	||(1>sscanf(argv[++iarg],"%lf", &damp[RAO_ROLL]))
#endif
	||(1>sscanf(argv[++iarg],"%lf", &waveheadingfromship))
	||(1>sscanf(argv[++iarg],"%f",  &(test_spectra.frequency)))
       )
      return(usage(iarg, argv));

    /* fill out wave test_spectra: */
    wave_number = test_spectra.frequency * test_spectra.frequency 
                  / GRAVITY;
    if (wave_number > 0) printf("wavelength = %lf\n", 2*PI/wave_number);
    angle = waveheadingfromship * PI /180.;
    /* pm wave vectors are backwards: */
    test_spectra.wave_number[0] = -wave_number * sin(angle);
    test_spectra.wave_number[1] = -wave_number * cos(angle);

    /* fill in rao_veh: */

#ifndef NO_READER
    trk_sea_veh_ru_read(NULL, READER_DEFAULTS);
    tracked_rao_get_veh_invalid(&rao_veh);
    if (0!=(veh=trk_sea_get_veh_ru("name", READER_CHARPTR, veh_name_ru)))
    {    
        reader_pretty_print(veh, stdout);
        tracked_rao_get_veh(veh, &rao_veh);
    }
    else  return(usage(1,argv));
#endif /*not NO_READER*/

    tracked_rao_generic_check_dims(&rao_veh); /* fills in nominal values 
					       * when 0s*/
    tracked_rao_generic_get_rao_poles(&rao_veh);

#ifdef NO_READER
    /* adjust pole damping factors: */
    for (d=0; d<N_RAO_DIM; d++)
      rao_veh.rao_poles[d].damp = damp[d];
#endif /*NO_READER*/

    tracked_rao_generic_get_inertias(&rao_veh);


    rao_veh.y_direction[1] = 1.;

    /* get results for 2 times: */
    /* PM phase already lags standard cos(w*t) by PI/2,
     * so include and extra +PI/2 jump forward in time
     * when calculating this waves phase lag (same as time_lag/period)
     * relative to PM wave. */
    tracked_rao_model(&rao_veh, 0.5*PI/test_spectra.frequency, NULL,
		      tracked_rao_test_spectra_iter,
		      results_cos);
    tracked_rao_model(&rao_veh, PI/test_spectra.frequency, NULL,
		      tracked_rao_test_spectra_iter,
		      results_sin);
    for (d=0; d<N_RAO_DIM; d++)
    {
	/* check the following !!!, get phase right*/
	results_amp2[d] = results_cos[d]*results_cos[d]
	                  + results_sin[d]*results_sin[d];
	results_amp[d] = sqrt(results_amp2[d]);
	results_phase[d] = atan2(results_sin[d], results_cos[d]);
    }
    /* print RAOs with desired units: */
    printf("\nRAO RESULTS (all angles are in degrees): \n");
    printf(" DIM         w0    PHASE_LAG      AMP/meter       (AMP/ft)**2\n");
    printf("                   (degrees)     (m or deg)    (ft or deg)**2\n");
    for (d=0; d<N_RAO_DIM; d++)
    {
	printf(" %5s  %8g  %10g  ",
	       rao_dim_names[d], 
	       rao_veh.rao_poles[d].w0,
	       (180./PI)*results_phase[d]);
	if (d==RAO_Z)
	  printf("   %10g     %10g\n",
		 results_amp[d], results_amp2[d]);
	else
#define METERPERFOOT  ( 0.0254 /*meters/inch*/  * 12 /*inches/foot*/ )
	  printf("   %10g     %10g\n",
		 (180./PI)*results_amp[d],
		 (180./PI)*(180./PI)*(METERPERFOOT*METERPERFOOT)
		 * results_amp2[d]);
    }
}

/* to be used in a loop looking like:
 * for (tracked_rao_test_spectra_iter(iter,pos,0);
 *      tracked_rao_test_spectra_iter(iter,pos,&spectra); )
 *   ...
 */
static int32  tracked_rao_test_spectra_iter(
    SPECTRA_ITER      *iter,
    float64            pos[3],
    SEA_SPECTRAL_DATA_REC *spectra)
{
    if (!spectra)
    {
	iter->nwaves = 1;
	iter->nfreqs = 1;
	iter->iwave = -1;
	iter->ifreq = 0;
	return(1);
    }
    else
    {
	if (  (iter->ifreq >= iter->nfreqs)  
            ||(  (++(iter->iwave) >= iter->nwaves)
	       &&(++(iter->ifreq) >= iter->nfreqs)))
	  return(0);
	if (iter->iwave >= iter->nwaves)  iter->iwave = 0;

	memcpy(spectra, &test_spectra, sizeof(SEA_SPECTRAL_DATA_REC));
	return(1);
    }
}

#ifndef NO_READER
int32 yywrap(void)
{
  return(1);
}
#endif /*not NO_READER*/

#endif /*TRACKED_RAO_TEST*/

/**************************************************************/
