/****************************************************************************
*   File: trk_sink.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: For Sinking ships.
*       Created: Tue Jan 28 11:03:54 EST 1997
*       Author: skukolic
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

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

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

/* INCLUDES AND DECLARATIONS: */

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

#include <math.h>

#ifdef TRK_SINK_TEST
#define TRACKED_SINK_TEST
#endif /*TRK_SINK_TEST*/

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

#ifndef NO_MODSAF

#ifdef NO_READER
#undef NO_READER
#endif

#define STATIC  static

/* ModSAF includes */

#include "libtrk_local.h"
#include <libtime.h>
#include <libentity.h>
#include <libphysdb.h>
#include <libvecmat.h>
#include <libctdb.h>

#include <sys/time.h>
#include <libreader.h>
#include <libenvironment.h>
/*#include <libenvsea.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 SINK model defines, and structs */

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


typedef struct tracked_sink_vars {
                              /* variables marked C are critical
                               * for getting state from time  */
  float64  started;
  /* VEHICLE DEPENDANT CONSTANTS */
  /* for mass model */
  float64  len[3];            /*  width, length, height       */
  float64  mass0;             /*C mass with no water          */
  float64  max_fill_fraction; /*C fraction of width*length*height
                               *  availible for water         */
  float64  fill_fraction_rate;/*C (d/dt) fill_fraction 
                               *  should be computed from 
                               *  damage, but no time         */
  /* for sinking model */
  float64  drag_multiplier;   /*  multiplicative drag fudge 
                               *  factor */
  /* for orientation model */
  float64  rp_transition[2];  /*C target roll, pitch angles
                               *  for t_transition            */
  float64  rp_final[2];       /*C target roll, pitch angles
			       *  for t_final                 */
  float64  rp_max_rate[2];    /*C max time derivative of roll
                               *  and pitch angles */
  /* z extra */
  float64  extra_z_floor;     /*C effective_z_floor = z_floor_external + this*/
  float64  z_offset;          /*C z_out = z_bottom + z_offset */

  /* COMPUTED CONSTANTS */
  /* for mass model */
  float64  mass_water;        /*C total_volume * WATER_DENSITY*/
  float64  fill_fraction_critical;
                              /*  fraction at transition to sinking*/
  /* SITUATION DEPENDENT CONSTANTS */
  /* z extra */
  float64  z_ocean, z_floor;  /*  z pos of ocean, & hard floor*/
  /* for sinking model */
  float64  drag_coeff;    /*  force_drag = drag_coeff*vel**2  */
                          /*  drag_coeff = 2*water_density*Area*fudge */
  float64  dzdt_max;      /*C max |dz/dt|                     */
  float64  t_const;       /*C integration constant for dz/dt  */
  float64  z_const;       /*C integration constant for z(t)   */
  /* computed times */
  float64  t_transition;  /*C when transition to sinking      */
  float64  t_final;       /*C when hit bottom                 */
  
  /* DYNAMICAL VARIABLES */
  float64  t_previous;    /*C time at which vars were valid   */
  /* mass model */
  /*float64  fill_fraction;*/ /*  how much of the vehicle's volume is
                               *  currently filled with water     */
  /*float64  mass;*/          /*  total mass, including water     */
  /*float64  is_sinking;*/    /*  0 => no (taking water), 1 => yes*/
  /* sinking model (or during mass model) */
  float64  z, dzdt;       /*  z position and z velocity       */

  /* orientation model */
  float64  roll_pitch[2]; /*C roll, pitch */
} TRACKED_SINK_VARS;


/**************************************************************/
/* SINK 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);

/* Fill in invalid values */
STATIC int32 tracked_sink_get_veh_invalid(
    TRACKED_SINK_VARS *sink);

/* Fill in the initial sink fields from the vehicle description, from
 * the reader file */
STATIC int32 tracked_sink_get_veh(
    READER_UNION      *veh,				  
    TRACKED_SINK_VARS *sink);

/* Fill in defaults for fields not already filled in */
STATIC int32 tracked_sink_get_veh_defaults(
    TRACKED_SINK_VARS *sink);

#endif /*NO_READER*/

#ifndef NO_MODSAF

int32  tracked_sink_init(
    char  *data_path,
    uint32 flags);

void  tracked_sink_add(
    int32         vehicle_id, 
    TRACKED_VARS *tracked, 
    float64       dt);

/* Fill in the sink mass and lenght fields from physdb,
 * if they have not already been filled in */
static int32 tracked_sink_get_veh_dims(
    int32          vehicle_id,
    TRACKED_SINK_VARS *sink);

/* Test to see if over water */
static int32 tracked_over_water(
    float64   pos[XYZC], 
    int32     vehicle_id);

/* Get sea-level, and hard floor z positions */
static int32 tracked_sink_get_z_ocean_z_floor(
    float64   pos[XYZC], 
    int32     vehicle_id,
    float64  *z_ocean,
    float64  *z_floor);


#endif /*not NO_MODSAF*/


/* tracked_sink_start()
 * After the vehicle-constants have been filled in,
 * this computes the rest the situation-dependant constants,
 * and the initial state. */
STATIC int32  tracked_sink_start(
    TRACKED_SINK_VARS *sink,
    float64        z_ocean,
    float64        z_floor,
    float64        time,
    float64        fill_fraction,  /* can be zero */
    float64        z,      /* z, and dzdt are only needed if already sinking */
    float64        dzdt,
    float64        roll,
    float64        pitch,
    float64        final_roll,  /* if > -7 radians then overrides defaults */
    float64        final_pitch);/* if > -7 radians then overrides defaults */

/* tracked_sink_state()
 * Compute sink state at a later time */
STATIC int32  tracked_sink_state(
    TRACKED_SINK_VARS *sink,
    float64        time,
    float64        *fill_fraction,
    float64        *mass,
    float64        *is_sinking,
    float64        *z,
    float64        *dzdt,
    float64        *roll,
    float64        *pitch);

static float64 arctanh(float64 y);
static float64 log_cosh(float64 x);
static float64 inv_log_cosh(float64 y);


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

/* 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);
}


/* TRACKED_SINK SPECIFIC: */

/* Table for parsing reader file description of a vehicle into
 * the appropriate TRACKED_SINK_VARS fields.
 * This table will contain pairs: {reader_symbol_name, sink_vars_offset} 
 */
static struct veh_fields {
    char  *sym;
    int32  offset;
} tracked_sink_veh_fields[] = 
{
/* offsets will be stored as float64 array references */
#define AOFFSET(FIELD) \
  (((int32)(&((TRACKED_SINK_VARS*)0)->FIELD))/sizeof(float64))
   { "mass",                  AOFFSET(mass0)},
   { "width",                 AOFFSET(len[0])},
   { "length",                AOFFSET(len[1])},
   { "height",                AOFFSET(len[2])},
   { "max_fill_fraction",     AOFFSET(max_fill_fraction) },
   { "fill_fraction_rate",    AOFFSET(fill_fraction_rate)},
   { "drag_multiplier",       AOFFSET(drag_multiplier)},
   { "transition_roll_angle", AOFFSET(rp_transition[0])},
   { "transition_pitch_angle",AOFFSET(rp_transition[1])},
   { "final_roll_angle",      AOFFSET(rp_final[0]) },
   { "final_pitch_angle",     AOFFSET(rp_final[1]) },
   { "max_roll_rate",         AOFFSET(rp_max_rate[0])},
   { "max_pitch_rate",        AOFFSET(rp_max_rate[1])},
   { "extra_z_floor",         AOFFSET(extra_z_floor) },
   { "extra_z_out_offset",    AOFFSET(z_offset) },
   { 0, 0 }
};

static int tracked_sink_veh_fields_inited = 0;

#define TRK_SEA_FIELD_INVALID (-98765.)


/* Fill in invalid values */
STATIC int32 tracked_sink_get_veh_invalid(
    TRACKED_SINK_VARS *sink)
{
    float64  *sinka, *sinkb;

    sinkb = (float64*)(&(sink->z_offset));
    for (sinka = (float64*)(&(sink->len[0])); sinka <= sinkb; ++sinka)
      *sinka = TRK_SEA_FIELD_INVALID;
    return(1);
}

/* Fill in the initial sink fields from the vehicle description, from
 * the reader file */
STATIC int32 tracked_sink_get_veh(
    READER_UNION      *veh,				  
    TRACKED_SINK_VARS *sink)
{
    READER_UNION  *field;
    struct veh_fields *fieldsp;
    float64  *sinka;

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

    sinka = (float64*)(sink);
    for (fieldsp = tracked_sink_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)
	      sinka[fieldsp->offset] = field[2].real;
	    else if (READER_UTYPE(field, 2)== READER_INTEGER)
	      sinka[fieldsp->offset] = field[2].integer;
	}
    }

    return(1);
}

/* Fill in defaults for fields not already filled in */
STATIC int32 tracked_sink_get_veh_defaults(
    TRACKED_SINK_VARS *sink)
{
#define IF_SET(VAR, VALUE) \
    if (TRK_SEA_FIELD_INVALID == (VAR))  (VAR) = (VALUE)

    IF_SET( sink->max_fill_fraction,    0.85  );
    IF_SET( sink->fill_fraction_rate,   1.0/(log(sink->mass0)/log(10.) + 2)  );
    IF_SET( sink->drag_multiplier,      1.0   );
    IF_SET( sink->rp_transition[0],     0.6   );
    IF_SET( sink->rp_transition[1],     0.2   );
    IF_SET( sink->rp_final[0],          1.0   );
    IF_SET( sink->rp_final[1],          0.1   );
    IF_SET( sink->rp_max_rate[0],       sink->fill_fraction_rate  );
    IF_SET( sink->rp_max_rate[1],       0.5 * sink->fill_fraction_rate  );
    IF_SET( sink->extra_z_floor,        0.);
    IF_SET( sink->z_offset,             0.);

    return(1);
}

#endif /*not NO_READER*/

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

/* MODSAF INTERFACE */

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

#ifndef NO_MODSAF

int32  tracked_sink_init(
    char  *data_path,
    uint32 flags)
{
    /* read the file trk_sea_veh.rdr */
    if (!trk_sea_veh_ru_read(data_path, flags))  return(0);
    return(1);
}

/**************************************************************/
/* tracked_sink_add() etc. */

void  tracked_sink_add(
    int32         vehicle_id, 
    TRACKED_VARS *tracked, 
    float64       dt)
{
    TRACKED_SINK_VARS *sink;
    float64        pos[XYZC];
    float64        vel[XYZC];
    float64        rot[XYZ][XYZ];
    float32        yaw32, pitch32, roll32;
    float64        yaw, pitch, roll;
    ObjectType     my_guise;
    float64        time;
    float64        fill_fraction, mass, is_sinking;
    READER_UNION   guise_ru, *veh_ru;
    int32    cell = ent_get_cell(vehicle_id);

    sink = (TRACKED_SINK_VARS*)(&(tracked->sink_state[0]));
    if (sink->started && (sink->t_previous >= sink->t_final)) return;

    ent_get_position_gcs(vehicle_id, pos);
    if (!tracked_over_water(pos, vehicle_id))  return;
    ent_get_velocity_gcs(vehicle_id, vel);
    ent_get_rotation_gcs(vehicle_id, rot, NULL);
    vmat3_rotation64_to_zdown_angles32(rot, &yaw32, &pitch32, &roll32);
    yaw = yaw32;  pitch = pitch32;  roll = roll32;
    guise_ru.integer = my_guise = ent_get_guise(vehicle_id, 0);

    time = 0.001*time_last_simulation_clock;
    if (!sink->started)
    {
	sink->started = 1;
	/* compute z_ocean, z_floor */
	tracked_sink_get_z_ocean_z_floor(pos, vehicle_id, 
					 &sink->z_ocean, &sink->z_floor);

	tracked_sink_get_veh_invalid(sink);
	if (0!=(veh_ru = trk_sea_get_veh_ru("guise",
					    READER_INTEGER, guise_ru)))
  	  tracked_sink_get_veh(veh_ru, sink);        /*from reader file entry*/
	tracked_sink_get_veh_dims(vehicle_id, sink); /* from physdb */
	tracked_sink_get_veh_defaults(sink);         /* computed */

	tracked_sink_start(sink, sink->z_ocean, sink->z_floor, time, 0.,
			   pos[Z], vel[Z], roll, pitch, -7., -7.);

    }
    tracked_sink_state(sink, time, &fill_fraction, &mass, &is_sinking,
		       &pos[Z], &vel[Z], &roll, &pitch);

    if (vehicle_id == tracked_rao_veh_id_debug)
	printf("veh %d: t= %lg, z= %lg, vz= %lg, pitch= %lg, roll= %lg\n",
	       vehicle_id, time, pos[Z], vel[Z], pitch, roll);

    vmat3_zdown_angles32_to_rotation64(yaw, pitch, roll, rot);
    ent_set_rotation_gcs(vehicle_id, rot, cell);
    ent_set_velocity_gcs(vehicle_id, vel);
    ent_set_position_gcs(vehicle_id, pos);
}

/* Fill in the sink mass and lenght fields from physdb,
 * if they have not already been filled in */
static int32 tracked_sink_get_veh_dims(
    int32          vehicle_id,
    TRACKED_SINK_VARS *sink)
{
    PHYSDB_DATA *pdb;
    int32        i;

    /* first get data from physdb: */
    pdb = ent_get_physdb(vehicle_id);
    if (sink->mass0 <= 0)  sink->mass0 = pdb->mass_kg;
    for (i=0; i<3; i++)  
      if (sink->len[i] <=0) 
	sink->len[i] = pdb->dimensions[i];
    /*sink->waterline = pdb->model_base_adjustment;*/
    return (1);
}

/* Test to see if over water */
static int32 tracked_over_water(
    float64   pos[XYZC], 
    int32     vehicle_id)
{
    CTDB            *ctdb;
    CTDB_ELEV_DATA maxelev;
    static CTDB_ELEV_QUALIFIER qual=(CTDB_INCL_WATER);

    ctdb = ent_get_ctdb(vehicle_id);
    ctdb_lookup_qual_elevation(ctdb, pos[X], pos[Y], qual, 
			       CTDB_NO_REF_ELEV, &maxelev);
    if (maxelev.tdbclass == CTDB_FC_ILLEGAL)
      return(0);
    return(1);
}

/* Get sea-level, and hard floor z positions */
static int32 tracked_sink_get_z_ocean_z_floor(
    float64   pos[XYZC], 
    int32     vehicle_id,
    float64  *z_ocean,
    float64  *z_floor)
{
    CTDB            *ctdb;
    CTDB_ELEV_DATA   elev[CTDB_MAX_ELEVS];
    int32            water_found, ground_found, eidx;
    ENV_INTERFACE    ei;

    ctdb = ent_get_ctdb(vehicle_id);
    ctdb_lookup_elevation_mes(ctdb, pos[X], pos[Y], CTDB_USE_DEFAULT, elev);

    water_found = ground_found = 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_SKIN)
	    {
		*z_floor = elev[eidx].z;
		ground_found = 1;
	    }
	    else if (elev[eidx].subclass == CTDB_FS_WATER)
	    {
		*z_ocean = elev[eidx].z;
		water_found = 1;
	    }
	}
	if (water_found && ground_found)  break;
    }
    if (!(water_found && ground_found))  return(0);

    bzero(&ei, sizeof(ENV_INTERFACE));
    bcopy(pos, &ei.u.sea_tide_level.input_vector, 4*sizeof(float64));
    if (env_get(ENV_SEA_TIDE_LEVEL, &ei) != -1)
      *z_ocean += ei.u.sea_tide_level.output_value;

    return(1);
}


#endif /*not NO_MODSAF*/

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

/* THIS SINK MODEL: */

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

/* tracked_sink_start()
 * After the vehicle-constants have been filled in,
 * this computes the rest the situation-dependant constants,
 * and the initial state. */
STATIC int32  tracked_sink_start(
    TRACKED_SINK_VARS *sink,
    float64        z_ocean,
    float64        z_floor,
    float64        time,
    float64        fill_fraction,  /* can be zero */
    float64        z,      /* z, and dzdt are only needed if already sinking */
    float64        dzdt,
    float64        roll,
    float64        pitch,
    float64        final_roll,  /* if > -7 radians then overrides defaults */
    float64        final_pitch) /* if > -7 radians then overrides defaults */
{
    float64  critical_fill_fraction;
    float64  t_1, z_1, dzdt_1;
    float64  mass_max;
    float64  a, c; /* intermediates in z, dzdt calcs */

    sink->z_ocean       = z_ocean;
    sink->z_floor       = z_floor += sink->extra_z_floor;
    sink->t_previous    = time;
    sink->roll_pitch[0] = roll;
    sink->roll_pitch[0] = pitch;
    sink->z             = z;
    sink->dzdt          = dzdt;
    if (final_roll  > -7)  sink->rp_final[0] = final_roll;
    if (final_pitch > -7)  sink->rp_final[1] = final_pitch;

    /* mass model initial computations */
    /* find t_transition (when can no longer float) */
    if (fill_fraction > sink->max_fill_fraction)
      fill_fraction = sink->max_fill_fraction;
    /* critical transition occurs when
     * when mass(t) > water_density * ship_volume,
     * mass(t) = mass0 + water_density * ship_volume * fill_fraction
     * => critical fill_fraction = 1 - mass0/(water_density * ship_volume) */
    sink->mass_water = WATER_DENSITY * sink->len[0] * sink->len[1]
                       * sink->len[2];
    sink->fill_fraction_critical 
      = 1 - sink->mass0 / (sink->mass_water);

    sink->t_transition = time 
      + (sink->fill_fraction_critical - fill_fraction)
	/ sink->fill_fraction_rate;

    /* check for t_final (when hits bottom) before t_transition */
    /* fill_fraction(t) = critical_fill_fraction + (t-t_transition)*fill_rate
     * floating depth(t) =  ship_height * fill_fraction(t)
     * z_ocean - z_floor = depth(t_final)
     * (If hits bottom before t_transition!!:)
     * => t_final = t_transition 
     *              + (((z_ocean - z_floor)/ship_height) 
     *                 - critical_fill_fraction)
     *                / fill_rate 
     */
    sink->t_final /* only if t_final occurs before t_transition */
      = sink->t_transition
	+ (((z_ocean - z_floor)/sink->len[2]) - sink->fill_fraction_critical)
	  / sink->fill_fraction_rate;
    if (sink->t_final <= sink->t_transition)
      return(1);

    /* starting ship sinking initial computations */
    /* find z_1, dzdt_1, at t_1, which is either now or
     * at the time of the future transition to sinking */
    if (time <= sink->t_transition) 
    {
	t_1    = sink->t_transition;
	z_1    = z_ocean - sink->len[2];
	dzdt_1 = - sink->len[2] * sink->fill_fraction_rate;
    }
    else
    {
	t_1    = time;
	z_1    = sink->z;
	dzdt_1 = sink->dzdt;
    }
    sink->drag_coeff = 2 * WATER_DENSITY * sink->len[0] * sink->len[1]
                       * sink->drag_multiplier;
    mass_max = sink->mass0 
               + sink->max_fill_fraction * sink->len[0] * sink->len[1]
                 * sink->len[2] * WATER_DENSITY;
    /* check for divide by zero */
    if (!((sink->drag_coeff <= 0) || (mass_max <= 0)))
      sink->dzdt_max = sqrt(mass_max * GRAVITY / sink->drag_coeff);
    else
      sink->dzdt_max = 10.;

    sink->t_const = t_1 
                   - (sink->dzdt_max/GRAVITY)*arctanh(-dzdt_1/sink->dzdt_max);
    a = GRAVITY * (t_1 - sink->t_const) / sink->dzdt_max;
    c = sink->dzdt_max * sink->dzdt_max / GRAVITY;
    sink->z_const = z_1  +  c * log_cosh( a );
    /* t_final from above equation, with z_floor instead of z_1, solved for t*/
    sink->t_final = 
      sink->t_const 
      + (sink->dzdt_max/GRAVITY)
	* inv_log_cosh( (sink->z_const - sink->z_floor) / c) ;
    return (1);
}


/* tracked_sink_state()
 * Compute sink state at a later time */
STATIC int32  tracked_sink_state(
    TRACKED_SINK_VARS *sink,
    float64        time,
    float64        *fill_fraction,
    float64        *mass,
    float64        *is_sinking,
    float64        *z,
    float64        *dzdt,
    float64        *roll,
    float64        *pitch)
{
    int32    irp;        /* roll or pitch index */
    float64  *rp_target/*[2]*/, rp_t_target;
    float64  rp_rate[2]; /* time derivative, used in linear interp */
    float64  rp[2];
    float64  a, c; /* intermediates in z, dzdt calcs */
    float64  t_want;

    /* mass model */
    *fill_fraction = sink->fill_fraction_critical
                     + (time - sink->t_transition) * sink->fill_fraction_rate;
    if (*fill_fraction < 0) *fill_fraction = 0;
    if (*fill_fraction > sink->max_fill_fraction) 
      *fill_fraction = sink->max_fill_fraction;
    *mass = sink->mass0 + *fill_fraction * sink->mass_water;
    *is_sinking = (time >= sink->t_transition);

    /* z(t) model */
    if (time >= sink->t_final)
    {
	/* final state */
	*z     = sink->z_floor;
	*dzdt  = 0;
    }
    else if (time <= sink->t_transition) 
    {
	/* z(t) is given by fill fraction */
	*z     = sink->z_ocean - sink->len[2] * (*fill_fraction);
	*dzdt  = - sink->len[2] * sink->fill_fraction_rate;

    }
    else
    {
	if (sink->t_previous < sink->t_transition)
	{
	    /* update to transtion time first */
	    tracked_sink_state(sink, sink->t_transition, fill_fraction, mass,
			   is_sinking, z, dzdt, roll, pitch);
	}
	a = GRAVITY * (time - sink->t_const) / sink->dzdt_max;
	c = sink->dzdt_max * sink->dzdt_max / GRAVITY;
	*dzdt = - sink->dzdt_max * tanh( a );
	*z = sink->z_const - c * log_cosh( a );
    }

    /* roll pitch model */
    /* linear interp for roll, pitch */
    if ((time < sink->t_final)||(sink->t_previous < sink->t_final))
    {
        t_want = (time < sink->t_final)?(time):(sink->t_final);
	if ((t_want < sink->t_transition) 
	    && (sink->t_transition < sink->t_final))
	{
	    rp_target   = &(sink->rp_transition[0]);
	    rp_t_target = sink->t_transition;
	} 
	else 
	{
	    rp_target   = &(sink->rp_final[0]);
	    rp_t_target = sink->t_final;
	}
	for (irp=0; irp<2; irp++)
	{
	    rp_rate[irp] = (rp_target[irp] - sink->roll_pitch[irp])
	      /(rp_t_target - sink->t_previous);
	    /* check that rate is not to fast */
	    if (fabs(rp_rate[irp]) > sink->rp_max_rate[irp])
	      rp_rate[irp] = sink->rp_max_rate[irp]
		             *((rp_rate[irp]<0)?(-1.):(1.));
	    rp[irp] = sink->roll_pitch[irp] 
	              + rp_rate[irp]*(t_want - sink->t_previous);
	}
	*roll  = rp[0];
	*pitch = rp[1];
    }

    /* save current state for use in next computation */
    sink->t_previous    = time;
    sink->z             = (*z += sink->z_offset);
    sink->dzdt          = *dzdt;
    sink->roll_pitch[0] = *roll;	   
    sink->roll_pitch[1] = *pitch;

    return (1);
}


static float64 arctanh(float64 y)
{
    float64  coshx, xdiff, x;

    if (y <= -1.)  return(-30.);
    if (y >=  1.)  return( 30.);
    /* Newton-Raphson iteration to solve y=tanh(x) for x 
     * dy/dx = 1/cosh(x)**2
     * x_new = x_old + (y - y_old)/(dy/dx) 
     */
    x = 0;
    /* max iter count tends to be about 33, more typically 6 */
    do
    {
        coshx = cosh(x);
	x += ( xdiff = ((y - tanh(x)) * coshx * coshx ) );
    }  
    while (fabs(xdiff) > 1.e-10) ;
    return(x);
}

#ifndef LOG2
#define LOG2  0.69314718055994529
#endif

static float64 log_cosh(float64 x)
{
    if (x > 20)  return(x - LOG2);
    else  return( log( cosh( x ) ) );
}

static float64 inv_log_cosh(float64 y)
{
    float64 x, xdiff;

    if (y <= 1.e-10)  return(0.);
    x = y + LOG2;
    do
    {
        if (x < 1.e-10)  return(0.);
	x += ( xdiff = ( (y - log_cosh(x)) / tanh(x) ) );
    } while (fabs(xdiff) > 1.e-10);
    return(x);
}


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

/* SINK MODEL TEST PROGRAM: */

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

#ifdef TRACKED_SINK_TEST

/* test program for testing this sink model */

#include <stdio.h>

int32 usage(int32 iarg, char *argv[])
{
  if (iarg >= 1)
    fprintf(stderr,"tracked_sink_test bad argument argv[%d] = \"%s\"\n",
	    iarg, argv[iarg]);
#define PF(S) fprintf(stderr, S)
  PF("Usage: testsink  \\\n");
#ifndef NO_READER
  PF("        vehicle_name \\\n");
#else /*NO_READER*/
  PF("        ship_width length height  mass0 \\\n");
  PF("        max_fill_fraction fill_fraction_rate drag_multiplier \\\n");
  PF("        roll_trans  roll_final  roll_max_rate \\\n");
  PF("        pitch_trans pitch_final pitch_max_rate \\\n");
#endif /*not NO_READER*/
  PF("        z_floor start_fill_fraction start_z start_dzdt start_roll start_pitch \\\n");
  PF("        t_max, delta_t\n");
  return(0);
}

int32 main(int32 argc, char *argv[])
{
    TRACKED_SINK_VARS  sink;
#ifndef NO_READER
    READER_UNION  veh_name_ru, *veh;
#endif
    int32    iarg = 0;
    float64  time, t_max, delta_t;
    float64  fill_fraction, mass, is_sinking, z, dzdt, roll, pitch;


#ifdef NO_READER
    if (  (argc < 22)
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.len[0]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.len[1]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.len[2]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.mass0))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.max_fill_fraction))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.fill_fraction_rate))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.drag_multiplier))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_transition[0]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_final[0]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_max_rate[0]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_transition[1]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_final[1]))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.rp_max_rate[1]))
#else
    if (  (argc < 10)
        ||(!(veh_name_ru.charptr = argv[++iarg]))
#endif /*NO_READER*/
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.z_floor))
        ||(1!=sscanf(argv[++iarg],"%lf", &fill_fraction))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.z))
        ||(1!=sscanf(argv[++iarg],"%lf", &sink.dzdt))
        ||(1!=sscanf(argv[++iarg],"%lf", &roll))
        ||(1!=sscanf(argv[++iarg],"%lf", &pitch))
        ||(1!=sscanf(argv[++iarg],"%lf", &t_max))
        ||(1!=sscanf(argv[++iarg],"%lf", &delta_t))
       )
      return(usage(iarg, argv));

#ifndef NO_READER
    trk_sea_veh_ru_read(NULL, READER_DEFAULTS);
    tracked_sink_get_veh_invalid(&sink);
    if (0!=(veh=trk_sea_get_veh_ru("name", READER_CHARPTR, veh_name_ru)))
      tracked_sink_get_veh(veh, &sink);
    else  return(usage(1,argv));
    tracked_sink_get_veh_defaults(&sink);
#endif /*not NO_READER*/
    
    tracked_sink_start(&sink, /*z_ocean*/ 0., sink.z_floor, /*time*/ 0.,
		   fill_fraction, sink.z, sink.dzdt,
                   roll, pitch, -7., -7.);
    printf("  t     fract      mass  sinking      z      dzdt    roll   pitch\n");
    for (time = 0; time <= t_max; time += delta_t) {
      tracked_sink_state(&sink, time, 
		     &fill_fraction, &mass, &is_sinking, &z, &dzdt, 
		     &roll, &pitch);
      printf("%7.3lf %0.4lf %9.4lg    %1d    %8.3lf %9.3lf %6.3lf %6.3lf\n",
             time, fill_fraction, mass, (int)(is_sinking), 
	     z, dzdt, roll, pitch);
    }
    return(0);
}

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

#endif /*TRACKED_SINK_TEST*/

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