/****************************************************************************
*   File: trk_tick.c                                                        *
*                                                                           *
*       Copyright 1995 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: Tick routine                                              *
*       Created: Mon Jul 13 1992                                            *
*       Author: jesmith                                                     *
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "libtrk_local.h"
#include <libentity.h>
#include <libenvironment.h>
#include <librandom.h>
#include <libctdb.h>
#include <libtime.h>
#include <libphysdb.h>
#include <libvecmat.h>
#include <libsubcomp.h>
#include <libsched.h>
#include <libcomponents.h>
#include <libhulls.h>
#include <libcollision.h>
#include <libconstants.h>
#include <veh_appear.h>
#include <p_safmodels.h>
#include <math.h>
#include <libgeometry.h>
#include <stdext.h> /*common/include/global*/
#include <stdstring.h>
#include <libdeployables.h>
#include <libreader.h>
#include <libenvironment.h>
#include <mun_type.h>
#include <libhandoff.h>
#include <env_type.h>
#include <libalert.h>
#include <libarty.h>

#define NEARLY_ZERO(x,err)  (((x) > -err) && ((x) < err))
#define CLOSE_ENOUGH(x,y)   (((x) <= y + 1) && ((x) >= y - 1))
#define SGN(x)              ((x) >= 0 ? 1 : -1)

/* The amount of time we slide when somebody smashes into us.  We go
 * ahead a compute a collision velocity based on momentum, so this
 * shouldn't look too fake.
 */
#define SLIDE_TIME 0.5  /* seconds */

/* Number of milliseconds between checks to see if the hull is within
 * a hydrology abstract feature.
 */
#define CHECK_PERIOD 10000  /* milliseconds */

/* The longest tick we trust linear dynamics when using external control.
 */
#define MAX_INTERNAL_TICK_SECONDS 1000.0

#define PI_4 0.7853990675

/* CCTT Mobility Model X Constants */
#define CCTTX_HEADING_ERROR_RATE_TAU_1          0.8
#define CCTTX_HEADING_ERROR_RATE_TAU_2          0.2
#define CCTTX_MU_COEFF_FACTOR                   2.0
#define CCTTX_TANGENT_SLIP_SPEED_CONSTANT       1.0

static int32 first_tick;

static HULLS_STATS *trkStats = NULL;

static int32 add_wave_influence(int32 vehicle_id, 
				TRACKED_VARS *tracked, float64 dt,
				int32 cell,
				float64 position[3], 
				float64 velocity[3],
				float64 hull_to_world[3][3],
		   		CTDB_MES_NODE_KEY mes_key);
static void skew(int32 vehicle_id);
static void get_soil(int32 vehicle_id, TRACKED_VARS *tracked, CTDB *ctdb);
static void flat_2D(float64 x, float64 y, float64 *xout, float64 *yout);

static int32 trk_in_trouble_slope(
    float64 soil_slope,
    TRACKED_VARS  *tracked,
    float64       position[XYZC],
    struct tracked_soils *soil);

static int32 trk_wet_weather_on(
    TRACKED_VARS        *tracked,
    float64             position[XYZC]);

static int32 trk_simnet_to_cctt_soil_mapping(
    TRACKED_VARS  *tracked,
    float64       position[XYZC],
    int32         soil_type);

static float64 trk_get_max_speed(
    int32 cctt_soil);

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

static void check_deployed(
    int32                 vehicle_id,   
    float64              *max_mps,   
    struct tracked_soils *soil);    

static int32 cctt_get_index(
    float64  value,   
    float64  min,   
    float64  max,   
    int32    n_values);    


/* This function uses the vehicle ID to check if there is a attachment which
 * affects vehicle speed.  A more expandable implementation  way is to have
 * a second set of "soil speeds" for vehicles with deployables.
 */
	
static void check_deployed(
    int32                 vehicle_id,   
    float64              *max_mps,   
    struct tracked_soils *soil)    
{
    char *deploy_name[ENT_MAX_PARTS];
    int32 comp;
    int32 is_attached;
    int32 is_valid;
    int32 is_active;
    int32 is_error;
    int32 type;
    int32 j, deployed;
    READER_UNION *speed_table, *soil_table;
    char whole_name[DEPLOYABLES_MAX_NAME_LENGTH+5] = "part_";
    int32 plow_type;
    uint32 the_default;

    /* check to see if there is a plow component attached to the entity. 
     * which can either be artic category "plow" or "dirtmover".
     */

    if (ent_get_artics_in_category(vehicle_id, tracked_plow_cat, 
				   deploy_name) ||
	ent_get_artics_in_category(vehicle_id, tracked_dirtmover_cat, 
				   deploy_name))
    {      
        /* special case for the FWMR and moldboard. if the plow is a FWMR
	 * then we really want to get the status of the moldboard that it
         * is attached to rather than the FWMR.  Note that we still use the
	 * FWMR parametric data in this case..
	 */

	if (deploy_name[0] == tracked_fwmr_symbol)
	    comp = cmpnt_locate(vehicle_id, tracked_moldboard_symbol);
	else	    
	    comp = cmpnt_locate(vehicle_id, deploy_name[0]);

	
	/* get information about plow.. */
	DEPLOYABLES_GET_POSITION(vehicle_id, comp,
				 &is_valid, &is_active, &is_error);

	/* check if it is deployed or stowed */
	deployed = (is_valid && is_active);

	/* get the part number from the name */
	strcpy(whole_name+5, deploy_name[0]);
	plow_type = const_name_to_value(reader_get_symbol(whole_name));

        /* adjust the speed depending upon the plow data from the
         * parametric data for this vehicle..
	 */

	/* look for the plow type.. */
	for (j=0; j<soil->n_plows; j++)
	  if (soil->plow_type[j] == plow_type) break;
	
	/* if the plow type was found, then adjust the speed..*/
	if (j < soil->n_plows)
	{
	    if (*max_mps > soil->plow_data[j][deployed])
	      *max_mps = soil->plow_data[j][deployed];
	} 
	else
	  fprintf(stderr, "libtracked: plow speed data for %s is not "
		          "specified for soil type %d.\n", 
		          deploy_name[0], soil->soil_type);

    } 

    /* now look for bridge components attached to this entity 
     * note that it is possible to have a bridge and a plow, so 
     * we should check both of these.. 
     */
    
    if (ent_get_artics_in_category(vehicle_id,tracked_bridge_cat,deploy_name))
    {
	speed_table = reader_find_tag(deploy_name[0],
				      tracked_speeds,
				      READER_UNTAGGED, READER_NO_ERRORS);
	
	if (speed_table && (*max_mps > speed_table[2].real))
	  *max_mps = speed_table[2].real;
    }
}



void tracked_tick(
    int32    vehicle_id,   
    uint32   interval_since_last_tick,   
    ADDRESS  user_data)    
{
    TRACKED_VARS *tracked = (TRACKED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  tracked_user_data_handle);

    float64 position[XYZC];
    float64 velocity[XYZC];
    float64 direction[XYZC];
    float64 dx, dy, odx, ody;
    float64 rot[XY][XY];
    float64 hull_to_world[XYZ][XYZ];
    float64 bridge_dir[XYZ];
    float64 bridge_pos[XYZ];
    
    int32   still, right_direction;
    float64 speed, desired_speed, desired_acceleration;
    struct  tracked_soils *soil;
    int32   new_soil;
    float64 max_mps, dz;
    float64 turn_angle, max_turn;
    float64 dt;
    float64 cross, dot;
    float64 slope, temp_vec[2], dist_from_center;

    PHYSDB_DATA *pdb, *bridge_pdb;
    uint32  old_appear, dust;
    float64 temp_z;
    int32 num_lanes, i;
    float64 dist, temp_dist;
    CTDB_MES_NODE_KEY mes_key;
    int32 mes_key_cell;
    float64 ccttx_tangent_slip_speed = 0.0;
    float64 slope_sin, soil_slope, veh_max_speed;
    int32  cctt_soil, cctt_vehicle_class;
    int32        cctt_index[10];  
    CTDB    *ctdb = ent_get_ctdb(vehicle_id);
    int32    cell = ent_get_cell(vehicle_id);

    if (!tracked)		/* Passive error detection */
      return;

    first_tick = (interval_since_last_tick == time_last_simulation_clock);

    dt = interval_since_last_tick / 1000.0;

    if (tracked->being_moved)   /* We're being towed, don't bother */
      /* $$$ add_wave_influence-like function (different RAO due to
       *     towing ) here.
       */
      return;

    if (!tracked->node_look_fcn(tracked,0,&mes_key_cell,&mes_key))
      ent_get_mes_key(vehicle_id);

    /* Nothing to do if the vehicle is dead */
    if (tracked->state == TRACKED_STATE_DEAD)
    {
	tracked_sink_add(vehicle_id, tracked, dt);
	return;
    }

    /* Derive speed and direction, if they aren't given explicitly */
    if ((tracked->control_state != TRACKED_EXPLICIT) &&
	(tracked->control_state != TRACKED_EXTERNAL))
      tick_mini_driver(vehicle_id, tracked, dt);

    /* Get needed variables.. all will be with respect to the
     * same cell (which will be position[CELL3D]
     */
    ent_get_position_gcs(vehicle_id, position);
    ent_get_velocity_gcs(vehicle_id, velocity);
    ent_get_direction_gcs(vehicle_id, direction);

    if(trkStats) hulls_statsTickStart(trkStats,                        
				      vehicle_id,    
				      tracked->speed,
				      position,
				      direction,
				      dt);

    /* Cache whether we are not moving */
    still = ((velocity[X] == 0.0) && (velocity[Y] == 0.0));

    pdb = ent_get_physdb(vehicle_id);

    if (!tracked->cells_initted)
    {
	/* this is a hack.  since we cannot find out the vehicles
	 * current direction in tracked_create like we used to (we
	 * shouldn't be calling another subclass's functions during
	 * our subclasses create routine)..  so, we initialize its cell
	 * to -1 in our create routine indicating that it hasn't yet
	 * been initialized.. we check for it here...
	 *
	 * this check will only occur the first tick and everytime
	 * a vehicle leaves a cell to enter a new one, so it isn't
	 * called all too often and isn't all that inefficient..  
	 */
	tracked->cells_initted = TRUE;
	VMAT4_VEC_COPY64(direction, tracked->desired_dir);
	tracked->node_keys.list_cell[0] = ent_get_cell(vehicle_id);
    }

    /* make sure that tracked->desired_dir is with respect to
     * the entities current cell.
     */
    if (cell != tracked->desired_dir[CELL3D])
    {
	/* perform the conversion in place.. */
	gcs_vec_to_vec64((int32)tracked->desired_dir[CELL3D],
			 tracked->desired_dir,
			 (int32)cell,
			 tracked->desired_dir);
	tracked->desired_dir[CELL3D] = cell;
    }

    if (!tracked->node_look_fcn(tracked,0,&mes_key_cell,&mes_key))
    {
	mes_key_cell = ent_get_cell(vehicle_id);
	mes_key = ent_get_mes_key(vehicle_id);
    }

    /* If we are not moving and our position changed, then we have been 
     * teleported somewhere and need to adjust our rotation to be placed
     * correctly on the terrain. 
     */

    if ((still) &&
	(!(CLOSE_ENOUGH(position[X], tracked->saved_position[X]))) ||
	(!(CLOSE_ENOUGH(position[Y], tracked->saved_position[Y]))) ||
	(!(position[CELL3D] == tracked->saved_position[CELL3D])))
    {

	/* Save the z value for possible restoration later */
 	temp_z = position[Z];
	
	/* clear the node key history list and set the current node
	 * key to be that of the current enclosure.
	 */
	mes_key = ent_get_mes_key(vehicle_id);
	tracked_clear_node_key_list(tracked,mes_key_cell,mes_key);
	tracked_set_current(tracked,mes_key_cell,mes_key);

	ctdb_place_vehicle_ml(ctdb, position[X], position[Y],
			      pdb->dimensions[Y], pdb->dimensions[X], 
			      pdb->mass_kg, 
			      tracked->desired_dir[X], tracked->desired_dir[Y],
			      position+Z, hull_to_world, &new_soil,
			      &mes_key);

	ent_set_rotation_gcs(vehicle_id, hull_to_world, cell);


	/* Restore the z for structures, since this may not necessarily
         * be the z at the center of mass of the structure 
         * which ctdb_place_vehicle returns. Also set soil latch, so
         * it doesn't perform unnecessary calculations and reset z again.
	 */
	if (VTAB_TYPES_MATCH(vtab_vehicle_type(vehicle_id),VTAB_STRUCTURE))
	{
	    position[Z] = temp_z;
	    tracked->soil_latch = 0;
	}
	
	/* save our position to that we can see if it changed later..
	 * (perhaps from a teleport of a vehicle. 
	 */
	VMAT4_VEC_COPY64(position, tracked->saved_position);
    }
    
    /* Cache whether we are facing the right direction.
     * Our direction must be parallel (cross = 0) and in the same
     * direction (dot > 0) as the desired.
     */

    cross = direction[X]*tracked->desired_dir[Y] - 
      direction[Y]*tracked->desired_dir[X];
    dot = direction[X]*tracked->desired_dir[X] + 
      direction[Y]*tracked->desired_dir[Y];

    right_direction = NEARLY_ZERO(cross,0.001) && (dot > 0.0);

    /* Grab the desired speed. */
    desired_speed = tracked->speed;

  /* the following section of code determines whether the vehicle
   * is operating normally or not. If not it will roll to a stop.
   */

    /* Are we out of gas? */
    if (tracked->state == TRACKED_STATE_NOGAS)
    {
	/* Has some kind soul given us more? */
 	if (scmp_get_amount(vehicle_id, munition_Fuel, 0) != 0.0)
	{
	    tracked->state = TRACKED_STATE_HEALTHY;
	    cmpnt_available(vehicle_id, SM_TrackedHull, TRUE);
	}
	else
	{
	    /* If we have no gas, we keep ticking dynamics just
	     * until we stop.
	     */
	    if (still)
	      return;

	    desired_speed = 0.0;
	}
    }

    /* Are we getting stuck? */
    if (tracked->state == TRACKED_STATE_STICKING)
    {
	/* Just like running out of gas.  Wait till we stop moving. */
	if (still)
	{
	    tracked->state = TRACKED_STATE_STUCK;

	    tracked->stuck_position[X] = position[X];
	    tracked->stuck_position[Y] = position[Y];
	    tracked->stuck_position[CELL3D] = position[CELL3D];
	    
	    cmpnt_available(vehicle_id, SM_TrackedHull, FALSE);
	    return;
	}
    }

    /* Are we stuck? */
    if (tracked->state == TRACKED_STATE_STUCK)
    {
	/* Have we been towed? */
	dx = position[X] - tracked->stuck_position[X];
	dy = position[Y] - tracked->stuck_position[Y];

 	if (!NEARLY_ZERO(dx,0.001) || !NEARLY_ZERO(dy,0.001) ||
	    (position[CELL3D] != tracked->stuck_position[CELL3D]))
	{
	    tracked->state = TRACKED_STATE_HEALTHY;
	    cmpnt_available(vehicle_id, SM_TrackedHull, TRUE);

	    /* Note that we don't know what we're on now. */
	    tracked->soil_latch = -1;
	}
	else
	  return;
    }

    if (tracked->state == TRACKED_STATE_DYING)
    {
	/* Just like running out of gas.  Wait till we stop moving. */
	if (still)
	{
	    if (tracked_mkill_cants_vehicle)
              skew(vehicle_id);

	    tracked->state = TRACKED_STATE_DEAD;
	    cmpnt_available(vehicle_id, SM_TrackedHull, FALSE);
	    return;
	}
	desired_speed = 0.0;
    }


    /* If the vehicle is not moving and doesn't want to move,
     * don't do all this expensive calculation, unless we need to
     * do a soil lookup (such as on the first tick).
     */
    if ((desired_speed == 0.0) && still && right_direction &&
	(tracked->soil_latch != -1))
    {
        if (scmp_decrement(vehicle_id, munition_Fuel, 0, .00017) == 0.0)
        {
           tracked->state = TRACKED_STATE_NOGAS;
           cmpnt_available(vehicle_id, SM_TrackedHull, FALSE);
        }

	(void)add_wave_influence(vehicle_id, tracked, dt, cell, NULL,
				 NULL, NULL, mes_key);
	return;
    }


    /* See if the bridge still exist */
    if (vtab_vehicle_type(tracked->bridge_id))
    {
	tracked->bridge_id = 0;
	tracked->soil_latch = -1;
    }


    /* Figure out if we are on a bridge or not.
     */
    if (tracked->bridge_id)
    {
	float64       bridge_poly[4][2];
	float64       bridge_parallel[2], bridge_perp[2];

	ent_get_position_gcscs(tracked->bridge_id, bridge_pos, 
			       (int32)position[CELL3D]);

	/* Compute the polygon which surrounds the bridge.
	 */
	bridge_pdb = ent_get_physdb(tracked->bridge_id);
	ent_get_direction_gcscs(tracked->bridge_id, bridge_dir,
				(int32)position[CELL3D]);

	vmat2_unit64(bridge_dir, bridge_parallel);

	bridge_perp[X] = bridge_parallel[Y];
	bridge_perp[Y] = -bridge_parallel[X];
	VMAT2_SCAL_VEC_MUL(bridge_pdb->dimensions[Y] * 0.5 + 1.0,
			   bridge_parallel, bridge_parallel);
	VMAT2_SCAL_VEC_MUL(bridge_pdb->dimensions[X] * 0.5 + 1.0,
			   bridge_perp, bridge_perp);

	VMAT2_VEC_ADD(bridge_pos, bridge_parallel, bridge_poly[0]);
	VMAT2_VEC_ADD(bridge_poly[0], bridge_perp, bridge_poly[0]);
	VMAT2_VEC_SUB(bridge_pos, bridge_parallel, bridge_poly[1]);
	VMAT2_VEC_ADD(bridge_poly[1], bridge_perp, bridge_poly[1]);
	VMAT2_VEC_SUB(bridge_pos, bridge_parallel, bridge_poly[2]);
	VMAT2_VEC_SUB(bridge_poly[2], bridge_perp, bridge_poly[2]);
	VMAT2_VEC_ADD(bridge_pos, bridge_parallel, bridge_poly[3]);
	VMAT2_VEC_SUB(bridge_poly[3], bridge_perp, bridge_poly[3]);

	/* Is the vehicle still on the bridge?
	 */
	tracked->on_bridge = geo_is_point_in_convex_polygon(position,
							    bridge_poly, 4)
	  == GEO_PT_INSIDE;

	if(tracked->on_bridge)
	{
            READER_UNION *speeds =
                reader_find_tag(reader_get_symbol("on_bridge"),
			        tracked_speeds,
                         	READER_UNTAGGED,
                                READER_NO_ERRORS);

	    tracked->soil_latch = 0;

            if (speeds)
	        desired_speed = NS_MIN(desired_speed, speeds[2].real);
	}

	if(!tracked->on_bridge &&
	   time_last_simulation_clock > tracked->bridge_collision_timeout)
	{
	    tracked->bridge_id = 0;
	    tracked->soil_latch = -1;
	}
    }

    /*
     * This section of code determines the speed we are traveling
     * at if the vehicle is operational.
     */

    /* How fast are we going right now? */
    speed = ent_get_speed(vehicle_id);

    /* Do we want to change gear? */
    if (tracked->current_gear != tracked->desired_gear)
    {
	if (speed < 0.1)
	{
	    tracked->current_gear = tracked->desired_gear;
	    if (tracked->current_gear == HULLS_GEAR_FORWARD)
	      tracked->node_look_fcn  = 
		(TRACKED_NODE_KEY_LOOK_FCN)tracked_node_key_look_ahead;
	    else
	      tracked->node_look_fcn  = 
		(TRACKED_NODE_KEY_LOOK_FCN)tracked_node_key_look_back;	    
	}
	else /* Don't allow gear change until we stop */
	  desired_speed = 0.0;
    }

    /* Make sure we know what soil type we're using */
    if (tracked->soil_latch == -1)
      get_soil(vehicle_id, tracked, ctdb);

    /* Get the parameters for the soil we're on. */
    soil = tracked->params->soils+tracked->soil_latch;


    /* See if we can't move through this stuff. */
    if (soil->max_speeds_mps[TRACKED_MAX_FORWARD] == 0.0)
    {
	tracked->state = TRACKED_STATE_STICKING;
	DEBUG_TRACKED(("Vehicle %d: About to get stuck in soil type %d\n",
		       vehicle_id, soil->soil_type));
    }
    else
    {
	/* See if we coasted through */
	if (tracked->state == TRACKED_STATE_STICKING)
	  tracked->state = TRACKED_STATE_HEALTHY;
    }

    /* Get slope */
    slope_sin = direction[Z];
    soil_slope = asin(fabs(slope_sin));
    soil_slope = RAD_TO_DEG(soil_slope);
    
    if (tracked_mobility || tracked_mobility_plus_allow_env)
    {
	cctt_soil =  trk_simnet_to_cctt_soil_mapping(tracked, position,
						     soil->soil_type);
	cctt_vehicle_class = 1;
	cctt_index[0] = cctt_vehicle_class;
	cctt_soil -= 1;
	if (cctt_soil < 0)
	  cctt_soil = 0;
	
	cctt_index[1] = cctt_soil;
	
	veh_max_speed = 
	  tracked_cctt_get_table_value(&tracked_cctt_table_max_speed,
				       cctt_index);

	if (desired_speed > veh_max_speed)
	  desired_speed = veh_max_speed; 
    }

    if ((tracked_mobility || tracked_mobility_plus_allow_env) &&
	(tracked->state != TRACKED_STATE_START_TROUBLE) &&
	(tracked->state != TRACKED_STATE_IN_TROUBLE))
    {
	if(trk_in_trouble_slope(soil_slope, tracked, 
				position, soil))
	{
	    tracked->state = TRACKED_STATE_START_TROUBLE;
	    tracked->waiting_in_trouble_state = time_last_simulation_clock;
	    tracked->saved_desired_speed = desired_speed;
	}
    }

    if (tracked->state == TRACKED_STATE_START_TROUBLE)
    {
	if ((tracked->waiting_in_trouble_state + 7000) > 
	    time_last_simulation_clock)
	{
	    desired_speed = 0.0;
	}
	else
	{
	    tracked->state = TRACKED_STATE_IN_TROUBLE;
	}
    }

    if(tracked->state == TRACKED_STATE_IN_TROUBLE)
    {
	if (trk_in_trouble_slope(soil_slope, tracked,
				 position, soil))
	{
	    if ((tracked->waiting_in_trouble_state + 
		 tracked->params->in_trouble_limit) <
		(time_last_simulation_clock))
	    {
		desired_speed = tracked->saved_desired_speed;
		tracked->state = TRACKED_STATE_STICKING;
	    }
	}
	else
	  tracked->state = TRACKED_STATE_HEALTHY;
    }
    /* 
     * ModSAF Vehicle Dynmaics Section
     *
     *   This section computes the ModSAF Mobility Model dynamics.
     * 
     *        Input:
     *            tracked->desired_dir[X],[Y]  - desired direction
     *            desired_speed   - desired speed
     *
     *        Output:
     *            dx, dy          - direction
     *            speed           - speed
     */

    if (tracked->params->mobility_model == TRK_MODSAF_MOBILITY_MODEL)
    {

	/*
	 * Compute what the speed will be for this tick.
	 * Note that this is only an approximation, since we haven't yet
	 * determined what direction we'll be facing this tick.  However,
	 * the difference will be very small, and this saves significant
	 * time by avoiding an elevation lookup.
	 */
	
	/* How fast can we go on this soil? */
	if (tracked->current_gear == HULLS_GEAR_FORWARD)
	{
	    max_mps = soil->max_speeds_mps[TRACKED_MAX_FORWARD];
	    dz = direction[Z];
	    check_deployed(vehicle_id,&max_mps, soil);
	}
	else
	{
	    max_mps = soil->max_speeds_mps[TRACKED_MAX_REVERSE];
	    dz = -direction[Z];
	}

	/* Adjust for grade */
	if(!tracked->on_bridge)
	  if (dz > soil->max_climb_sin) /* Too steep */
	    max_mps = 0.0;
	  else if (dz > 0.0)	/* Not downhill */
	    max_mps *= 1.0 - sqrt(dz / soil->max_climb_sin); /* Magic */
	
	/* How fast do we want to go? */
	if (desired_speed > max_mps)
	  desired_speed = max_mps;
	
	/* If we want to change our speed, do as much as possible */
	if (speed != desired_speed)
	{
	    /* How much would we have to accelerate to make this speed? */
	    desired_acceleration = (desired_speed - speed) / dt;
	    
	    /* First do braking */
	    if (desired_acceleration < 0.0)
	    {
#if 0
		/* If we need to brake to avoid collisions it does
		 * not make sense to limit how much we brake. Perhaps
		 * we need to differentiate between emergengy and common
		 * braking.
		 */
			/* Only deccelerate as much as allowed */
			if ((tracked->max_accel != 0.0) &&
			    (-desired_acceleration > tracked->max_accel))
			  desired_acceleration = -tracked->max_accel;
#endif

		/* Can we slow down that fast? */
		if (-desired_acceleration < soil->max_decel_mps2)
		  speed = desired_speed;
		else
		  speed = speed - soil->max_decel_mps2 * dt;
	    }
	    else
	    {
		/* Only accelerate as much as allowed */
		if ((tracked->max_accel != 0.0) &&
		    (desired_acceleration > tracked->max_accel))
		  desired_acceleration = tracked->max_accel;
		
		/* Can we speed up that fast? */
		if (desired_acceleration < soil->max_accel_mps2)
		  speed = desired_speed;
		else
		  speed = speed + soil->max_accel_mps2 * dt;
	    }
	}

	/*
	 * Compute the turn accomplished during this tick
	 */
	
	/* Make our desired direction a 2D unit vector */
	flat_2D(tracked->desired_dir[X], tracked->desired_dir[Y], 
		&tracked->desired_dir[X], &tracked->desired_dir[Y]);
	
	/* Don't do anything expensive if we're already pointing the
	 * right way.
	 */
	if (right_direction)
	{
	    dx = tracked->desired_dir[X];
	    dy = tracked->desired_dir[Y];
	}
	else
	{
	    /* Get our current direction in a 2D unit vector */
	    flat_2D(direction[X], direction[Y], &dx, &dy);
	    
	    /* How much do I want to turn (in radians)?
	     * Both directions are unit vectors, so we can just use the
	     * acos of the dot product to find the angle, and the sign
	     * of the cross product to find left or right.  We already
	     * computed the cross product a while ago, and changing the
	     * magnitudes of the vectors did not affect its sign.
	     */
	    if (cross > 0) /* left */
	      turn_angle = acos(dx*tracked->desired_dir[X] +
				dy*tracked->desired_dir[Y]);
	    else
	      turn_angle = -acos(dx*tracked->desired_dir[X] + 
				 dy*tracked->desired_dir[Y]);
	    
	    /* Can I turn that much?  Compare to the lesser of what
	     * I can do and what I am allowed to do.
	     * Zero in tracked->max_turn means no limit from driver.
	     */
	    if ((tracked->max_turn == 0.0) ||
		(soil->max_turn_rps < tracked->max_turn))
	      max_turn = soil->max_turn_rps * dt;
	    else
	      max_turn = tracked->max_turn * dt;
	    
	    if ((turn_angle > -max_turn) && (turn_angle < max_turn))
	    {
		/* Adopt the new direction. */
		dx = tracked->desired_dir[X];
		dy = tracked->desired_dir[Y];
	    }
	    else
	    {
		/* Turn the maximum amount */
		if (turn_angle < 0)
		  max_turn = -max_turn;
		vmat2_primary_rotation64(sin(max_turn), cos(max_turn), rot);
		odx = dx, ody = dy;
		dx = odx * rot[X][X] + ody * rot[Y][X];
		dy = odx * rot[X][Y] + ody * rot[Y][Y];
	    }
	}
    }



    /* 
     * CCTT Vehicle Dynmaics Section
     *
     *     This section of code attempts to replicate as closely as possible
     * the dynamics algorithm as implementation in ADA in the CCTT Program. 
     * Every attempt was made to match algorithm flow, variable names and 
     * coding styles.   However, where ModSAF variables exist --- such as
     * speed, direction, velocity, etc. --- they were used in place of the
     * CCTT names.  
     *
     *        Input:
     *            tracked->desired_dir[X],[Y]  - desired direction
     *            desired_speed   - desired speed
     *
     *        Output:
     *            dx, dy          - direction
     *            speed           - speed
     *
     */

    else if (tracked->params->mobility_model == TRK_CCTT_MOBILITY_MODEL)
    {
	float64      cctt_slope;                   /* slope (radians) */
	float64      cctt_slope_sin;               /* sine of slope */
	float64      cctt_slope_cos;               /* cosine of slope */
	int32        cctt_vehicle_class;           /* CCTT vehicle class */
	int32        cctt_soil_type;               /* CCTT soil type */
	int32        cctt_index[10];               /* temporary index array */

	int32        cctt_local_gear;              /* local gear (FORWARD, REVERSE or STOP) */
	float64      cctt_local_commanded_speed;   /* commanded speed (meters/second) */
	float64      cctt_heading_error;           /* heading error (radians +/- PI) */
	float64      cctt_heading_error_rate;      /* heading error rate (radius/second) */
	float64      cctt_speed_error;             /* difference in desired and actual speed (meters/second) */
	float64      cctt_speed_error_limited_abs; /* limited, absolute value of speed error */
	float64      cctt_brake_pos_percent;       /* brake position percent (from 0.0 to 1.0) */
	float64      cctt_brake_linear_factor;     /* brake linear factor (TABLE LOOKUP) */
	float64      cctt_commanded_throttle_pos_rate;  /* desired throttle position rate (from 0.0 to 1.0) */
	float64      cctt_accel_mod;               /* modified acceleration value */
	float64      cctt_force_drive;             /* driving force (newtons) */
	float64      cctt_force_brake;             /* brake force (newtons) */
	float64      cctt_force_friction;          /* frictional force (newtons) */
	float64      cctt_force;                   /* sum of forces on the vehicle (newtons) */
	float64      cctt_weight_longitudinal;     /* weight along direction of motion (newtons) */
	float64      cctt_weight_vertical;         /* weight normal to direction of motion (newtons) */
	float64      cctt_mu_eff_coeff;            /* effective coefficient of friction */
	float64      cctt_max_turn_rate;           /* maximum turn rate (radians/second) (TABLE LOOKUP) */
	float64      cctt_max_speed;               /* maximum speed (meters/second) (TABLE LOOKUP) */
	ENV_INTERFACE cctt_env_interface;        /* environmental interface */
	CTDB_ABSTRACT_DATA *data;

    /* CCTT Initialize values */

        /* Get slope */
	cctt_slope_sin = direction[Z];
	cctt_slope     = asin(cctt_slope_sin);
	cctt_slope_cos = cos(cctt_slope);

        /* CCTT vehicle class */
	cctt_index[0] = (int32) ent_get_vehicle_class(vehicle_id);
	cctt_vehicle_class = (int32)
	  tracked_cctt_get_table_value(&tracked_cctt_table_vehicle_map, 
				       cctt_index);

        /* CCTT soil type */
        /* CCTT soil type */
	/* The CCTT soil type indices are assigned as follows:
	 *
	 * cctt_index[0] - subsurface saturation
	 * cctt_index[1] - surface wetness
	 * cctt_index[2] - soil type
	 */
	if(time_last_simulation_clock - tracked->last_check_time >
	   CHECK_PERIOD)
	{
	    tracked->last_check_time = time_last_simulation_clock;
	    tracked->within_hydro_surface =
	      ctdb_point_within_abstract(ctdb,
					 CTDB_ABSTRACT_HYDRO_SURFACE,
					 position[X], position[Y], &data) &&
	      data->hydro_surface.wetness;
	    tracked->within_hydro_subsurface =
	      ctdb_point_within_abstract(ctdb,
					 CTDB_ABSTRACT_HYDRO_SUBSURFACE,
					 position[X], position[Y], &data) &&
	      data->hydro_subsurface.wetness;
	}

	if(tracked->within_hydro_subsurface)
	  cctt_index[0] = 1;
	else
	  cctt_index[0] = 0;

	/* Get precipitation information */
	/* Set environmental interface structure */
	/* Set location */

	VMAT4_VEC_COPY64(position, 
			 cctt_env_interface.u.generic_vector_to_value.
			 input_vector);


	if (env_get(ENV_PRECIPITATION_RATE, &cctt_env_interface) == -1 &&
	    !tracked->within_hydro_surface)
	  cctt_index[1] = 0;
	else
	  {
	    /* if the precipitation rate is positive use the CCTT wet soil
             *  types 
	     */
	    if (cctt_env_interface.u.precipitation_rate.output_value > 0.0 ||
		tracked->within_hydro_surface)
	      cctt_index[1] = 1;
	    else
	      cctt_index[1] = 0;
	  }

	/* Map ModSAF to CCTT Soil Types */
	cctt_index[2] = (int32) soil->soil_type;

	cctt_soil_type =
	  (int32) tracked_cctt_get_table_value(&tracked_cctt_table_soil_map,
					       cctt_index);

	/* CCTT soil types are indexed from 1 to 30.  However, since we index
         * into the data tables from 0, we must subtract one from this index.
         * A 0 CCTT value means soil type unknown, in this case we print a
         * warning and assign 0 to CCTT type 1 
	 */
	cctt_soil_type -= 1;
	if (cctt_soil_type < 0)
	    cctt_soil_type = 0;

	/* CCTT Controller Section */

	/* Read maximum speed (meters/second) based on (1) Vehicle Class and
         * (2) Soil Type from CCTT data table 
	 */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_max_speed = tracked_cctt_get_table_value(
	    &tracked_cctt_table_max_speed, cctt_index);

	/* Determine Local Gear */
	cctt_local_gear = tracked->current_gear;

	/* Determine Commanded Speed */
	cctt_local_commanded_speed = desired_speed;
	if (cctt_local_commanded_speed > cctt_max_speed)
	  cctt_local_commanded_speed = cctt_max_speed;

	/* Project direction vectors into the xy plane */
	flat_2D(tracked->desired_dir[X], tracked->desired_dir[Y], 
		&tracked->desired_dir[X], &tracked->desired_dir[Y]);
	flat_2D(direction[X], direction[Y], &dx, &dy);

	/* Determine Heading Error */
	cctt_heading_error = 
	  vmat2e_angle_between_vectors64(dx, dy, 
					 tracked->desired_dir[X], 
					 tracked->desired_dir[Y]);
	
	/* Determine Heading Error Rate */
	cctt_heading_error_rate = tracked->params->heading_error_rate_tau*
	  cctt_heading_error;

	/* Read Maximum Turn Rate (radians/second) based on (1) Vehicle Class
         *  and (2) Soil Type from CCTT data tables 
	 */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_max_turn_rate = tracked_cctt_get_table_value(
	    &tracked_cctt_table_max_turn_rate, cctt_index);

	/* Limit Heading Error Rate */
	if (cctt_heading_error_rate > cctt_max_turn_rate)
	  cctt_heading_error_rate = cctt_max_turn_rate;
	else if (cctt_heading_error_rate < -cctt_max_turn_rate)
	  cctt_heading_error_rate = -cctt_max_turn_rate;

	/* Determine Turn Angle */
	turn_angle = cctt_heading_error_rate * dt;

	/* Compute New Angle */
	vmat2_primary_rotation64(sin(turn_angle), cos(turn_angle), rot);
	odx = dx, ody = dy;
	dx = odx * rot[X][X] + ody * rot[Y][X];
	dy = odx * rot[X][Y] + ody * rot[Y][Y];

	/* Determine Speed Error */
	cctt_speed_error = cctt_local_commanded_speed - speed;
	
	/* Limit Speed Error */
	if (fabs(cctt_speed_error) > cctt_max_speed)
	  cctt_speed_error_limited_abs = cctt_max_speed;
	else
	  cctt_speed_error_limited_abs = fabs(cctt_speed_error);

	/* Apply Brakes */
	if (cctt_speed_error < 0.0)
	{
	    if (cctt_max_speed > 0.0)
	      cctt_brake_pos_percent = 1.0 - 
		exp(-fabs(cctt_speed_error_limited_abs)/cctt_max_speed);
	    else
	      cctt_brake_pos_percent = 1.0;

	    tracked->commanded_throttle_pos = 0.0;
	}

	/* Apply Throttle */
	else
	{
            /* compute modified acceleration value */
	    cctt_accel_mod =  tracked->acceleration;

	    /* compute commanded throttle position rate */
	    cctt_commanded_throttle_pos_rate = 
               tracked->params->throttle_forward_loop_gain*
	      (SGN(cctt_speed_error)*cctt_speed_error_limited_abs 
	       - tracked->params->throttle_accel_feedback_gain*cctt_accel_mod);
	    
	    /* integrate throttle position */
	    tracked->commanded_throttle_pos += 
	      cctt_commanded_throttle_pos_rate*dt;

	    /* limit commanded throttle position */
	    if (tracked->commanded_throttle_pos > 1.0)
	      tracked->commanded_throttle_pos = 1.0;
	    else if (tracked->commanded_throttle_pos < 0.0)
	      tracked->commanded_throttle_pos = 0.0;

	    cctt_brake_pos_percent = 0.0;
	}
	    
	/* compute throttle position */
	if (cctt_local_commanded_speed > 0.0)
	  tracked->throttle_pos += tracked->params->tau_throttle_inv*
	    dt*(tracked->commanded_throttle_pos - tracked->throttle_pos);
	else
	  tracked->throttle_pos = 0.0;

	/* limit throttle position */
	if (tracked->throttle_pos > 1.0)
	  tracked->throttle_pos = 1.0;
	else if (tracked->throttle_pos < 0.0)
	  tracked->throttle_pos = 0.0;

	/* CCTT Dynamics Section */

	/* Determine Driving Force */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_index[2] = cctt_get_index(tracked->throttle_pos, 0.0, 1.0, 
				       tracked->params->n_throttle_positions);
	cctt_index[3] = cctt_get_index(cctt_slope, 0.0, PI_4, 
				       tracked->params->n_slopes);
	cctt_index[4] = cctt_get_index(speed, 0.0, cctt_max_speed, 
				       tracked->params->n_speeds);
	cctt_force_drive = TRACKED_CCTT_GRAVITY*
	  tracked_cctt_get_table_value(&tracked_cctt_table_force_drive, 
				       cctt_index);

	/* Scale drive force for reverse direction */
	if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE)
	  cctt_force_drive *= tracked->params->reverse_drive_factor;

        /* Determine Braking Force */
	if ((speed > TRACKED_CCTT_THRESHOLD) && 
	    (cctt_brake_pos_percent > TRACKED_CCTT_THRESHOLD))
	{
	    cctt_index[0] = cctt_vehicle_class;
	    cctt_index[1] = cctt_soil_type;
	    cctt_brake_linear_factor = tracked_cctt_get_table_value(
		&tracked_cctt_table_force_brake, cctt_index);
	    cctt_force_brake = pdb->mass_kg * cctt_brake_pos_percent * 
	      cctt_brake_linear_factor;
	    cctt_force_brake = tracked_cctt_get_table_value(
		&tracked_cctt_table_force_brake, cctt_index);
	}
	else
	  cctt_force_brake = 0.0;

	/* Determine Gravitational Force */
	cctt_weight_longitudinal = pdb->mass_kg*TRACKED_CCTT_GRAVITY*
	  cctt_slope_sin;
	cctt_weight_vertical     = pdb->mass_kg*TRACKED_CCTT_GRAVITY*
	  cctt_slope_cos;

	/* Ignore Gravity Going Downhill */
	if (((cctt_slope < 0.0) && 
	     (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)) ||
	    ((cctt_slope > 0.0) && 
	     (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE)))
	{
	    cctt_weight_longitudinal = 0.0;
	    cctt_weight_vertical     = pdb->mass_kg*TRACKED_CCTT_GRAVITY;
	}
	  
	/* Determine Friction Force */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_mu_eff_coeff = tracked_cctt_get_table_value(
	    &tracked_cctt_table_friction, cctt_index);
	cctt_force_friction = cctt_mu_eff_coeff*cctt_weight_vertical;

	if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD))
	{
	    cctt_force_friction = 0.0;
	    cctt_weight_longitudinal = 0.0;
	}

	/* Sum Forces */
	if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)
	  cctt_force = cctt_force_drive - cctt_weight_longitudinal - 
	    cctt_force_friction - cctt_force_brake;
	else if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE)
	  cctt_force = -cctt_force_drive - cctt_weight_longitudinal +
	    cctt_force_friction + cctt_force_brake;
	else
	  cctt_force = 0.0;

	/* Compute Acceleration */
	tracked->acceleration = cctt_force/pdb->mass_kg;

	/* Compute Speed */
	if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)
	  speed += tracked->acceleration*dt;
	else
	  speed -= tracked->acceleration*dt;
	speed = fabs(speed);

	if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD))
	  speed = 0.0;
	else if (speed > cctt_max_speed)
	  speed = cctt_max_speed;
    }


    /* 
     * CCTT Vehicle Dynmaics X Section
     *
     *     This section of code attempts to replicate as closely as possible
     * the dynamics algorithm as implementation in ADA in the CCTT Program
     * with extensions for rotational inertia and tangential sliding.
     * Every attempt was made to match algorithm flow, variable names and 
     * coding styles.   However, where ModSAF variables exist --- such as
     * speed, direction, velocity, etc. --- they were used in place of the
     * CCTT names.  
     *
     *        Input:
     *            tracked->desired_dir[X],[Y]  - desired direction
     *            desired_speed   - desired speed
     *
     *        Output:
     *            dx, dy          - direction
     *            speed           - speed
     *
     */

    else
    {
	float64      cctt_slope;                   /* slope (radians) */
	float64      cctt_slope_sin;               /* sine of slope */
	float64      cctt_slope_cos;               /* cosine of slope */
	int32        cctt_vehicle_class;           /* CCTT vehicle class */
	int32        cctt_soil_type;               /* CCTT soil type */
	int32        cctt_index[10];               /* temporary index array */

	int32        cctt_local_gear;              /* local gear (FORWARD, REVERSE or STOP) */
	float64      cctt_local_commanded_speed;   /* commanded speed (meters/second) */
	float64      cctt_heading_error;           /* heading error (radians +/- PI) */
	float64      cctt_heading_error_accel;     /* heading error acceleration (radians/second^2) */
	float64      cctt_speed_error;             /* difference in desired and actual speed (meters/second) */
	float64      cctt_speed_error_limited_abs; /* limited, absolute value of speed error */
	float64      cctt_brake_pos_percent;       /* brake position percent (from 0.0 to 1.0) */
	float64      cctt_brake_linear_factor;     /* brake linear factor (TABLE LOOKUP) */
	float64      cctt_commanded_throttle_pos_rate;  /* desired throttle position rate (from 0.0 to 1.0) */
	float64      cctt_accel_mod;               /* modified acceleration value */
	float64      cctt_force_drive;             /* driving force (newtons) */
	float64      cctt_force_brake;             /* brake force (newtons) */
	float64      cctt_force_friction;          /* frictional force (newtons) */
	float64      cctt_force;                   /* sum of forces on the vehicle (newtons) */
	float64      cctt_weight_longitudinal;     /* weight along direction of motion (newtons) */
	float64      cctt_weight_vertical;         /* weight normal to direction of motion (newtons) */
	float64      cctt_mu_eff_coeff;            /* effective coefficient of friction */
	float64      cctt_max_turn_rate;           /* maximum turn rate (radians/second) (TABLE LOOKUP) */
	float64      cctt_max_speed;               /* maximum speed (meters/second) (TABLE LOOKUP) */
	ENV_INTERFACE cctt_env_interface;          /* environmental interface */
        float64      ccttx_tangent_angle;          /* tangent angle (used to determine tangential slipping) */
        float64      ccttx_tangent_angle_sin;      
        float64      ccttx_tangent_angle_cos;      
        float64      ccttx_tangent_angle_tan;      
	CTDB_ABSTRACT_DATA *data;

    /* CCTT Initialize values */

	/* Get physical parameters */
	pdb = ent_get_physdb(vehicle_id);

        /* Get slope */
	cctt_slope_sin = direction[Z];
	cctt_slope     = asin(cctt_slope_sin);
	cctt_slope_cos = cos(cctt_slope);

        /* CCTT vehicle class */
	cctt_index[0] = (int32) ent_get_vehicle_class(vehicle_id);
	cctt_vehicle_class = (int32) tracked_cctt_get_table_value(
	    &tracked_cctt_table_vehicle_map, cctt_index);

        /* CCTT soil type */
	/* The CCTT soil type indices are assigned as follows:
	 *
	 * cctt_index[0] - subsurface saturation
	 * cctt_index[1] - surface wetness
	 * cctt_index[2] - soil type
	 */
	if(time_last_simulation_clock - tracked->last_check_time >
	   CHECK_PERIOD)
	{
	    tracked->last_check_time = time_last_simulation_clock;
	    tracked->within_hydro_surface =
	      ctdb_point_within_abstract(ctdb,
					 CTDB_ABSTRACT_HYDRO_SURFACE,
					 position[X], position[Y], &data) &&
	      data->hydro_surface.wetness;
	    tracked->within_hydro_subsurface =
	      ctdb_point_within_abstract(ctdb,
					 CTDB_ABSTRACT_HYDRO_SUBSURFACE,
					 position[X], position[Y], &data) &&
	      data->hydro_subsurface.wetness;
	}

	if(tracked->within_hydro_subsurface)
	  cctt_index[0] = 1;
	else
	  cctt_index[0] = 0;

	/* Get precipitation information */
	/* Set environmental interface structure */
	/* Set location */

	VMAT4_VEC_COPY64(position, 
			 cctt_env_interface.u.generic_vector_to_value.
			 input_vector);


	if (env_get(ENV_PRECIPITATION_RATE, &cctt_env_interface) == -1 &&
	    !tracked->within_hydro_surface)
	  cctt_index[1] = 0;
	else
	  {
	    /* if the precipitation rate is positive use the CCTT wet soil
             * types 
	     */
	    if (cctt_env_interface.u.precipitation_rate.output_value > 0.0 ||
		tracked->within_hydro_surface)
	      cctt_index[1] = 1;
	    else
	      cctt_index[1] = 0;
	  }

	/* Map ModSAF to CCTT Soil Types */
	cctt_index[2] = (int32) soil->soil_type;

	cctt_soil_type =
	  (int32) tracked_cctt_get_table_value(&tracked_cctt_table_soil_map,
					       cctt_index);

	/* CCTT soil types are indexed from 1 to 30.  However, since we index
         *  into the data tables from 0, we must subtract one from this index.
         *  A 0 CCTT value means soil type unknown, in this case we print a
         *  warning and assign 0 to CCTT type 1 
	 */
	cctt_soil_type -= 1;
	if (cctt_soil_type < 0)
	    cctt_soil_type = 0;

	/* CCTT Controller Section */

	/* Read maximum speed (meters/second) based on (1) Vehicle Class and
         * (2) Soil Type from CCTT data table 
	 */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_max_speed = tracked_cctt_get_table_value(
	    &tracked_cctt_table_max_speed, cctt_index);

	/* Determine Local Gear */
	cctt_local_gear = tracked->current_gear;

	/* Determine Commanded Speed */
	cctt_local_commanded_speed = desired_speed;
	if (cctt_local_commanded_speed > cctt_max_speed)
	  cctt_local_commanded_speed = cctt_max_speed;

	/* Project direction vectors into the xy plane */
	flat_2D(tracked->desired_dir[X], tracked->desired_dir[Y], 
		&tracked->desired_dir[X], &tracked->desired_dir[Y]);
	flat_2D(direction[X], direction[Y], &dx, &dy);

	/* Determine Heading Error */
	cctt_heading_error = 
	  vmat2e_angle_between_vectors64(dx, dy, 
					 tracked->desired_dir[X], 
					 tracked->desired_dir[Y]);
	
	/* Determine heading error acceleration */
	cctt_heading_error_accel = -CCTTX_HEADING_ERROR_RATE_TAU_1*tracked->ccttx_heading_error_rate -
	                            CCTTX_HEADING_ERROR_RATE_TAU_2*cctt_heading_error;


	/* Determine heading error rate */
	tracked->ccttx_heading_error_rate += cctt_heading_error_accel*dt;


	/* Read Maximum Turn Rate (radians/second) based on (1) Vehicle Class
         * and (2) Soil Type from CCTT data tables 
	 */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_max_turn_rate = tracked_cctt_get_table_value(
	    &tracked_cctt_table_max_turn_rate, cctt_index);

	/* Limit Heading Error Rate */
	if (tracked->ccttx_heading_error_rate > cctt_max_turn_rate)
	  tracked->ccttx_heading_error_rate = cctt_max_turn_rate;
	else if (tracked->ccttx_heading_error_rate < -cctt_max_turn_rate)
	  tracked->ccttx_heading_error_rate = -cctt_max_turn_rate;

	/* Determine Turn Angle */
	turn_angle = -tracked->ccttx_heading_error_rate * dt;

	/* Compute New Angle */
	vmat2_primary_rotation64(sin(turn_angle), cos(turn_angle), rot);
	odx = dx, ody = dy;
	dx = odx * rot[X][X] + ody * rot[Y][X];
	dy = odx * rot[X][Y] + ody * rot[Y][Y];

	/* Determine Speed Error */
	cctt_speed_error = cctt_local_commanded_speed - speed;
	
	/* Limit Speed Error */
	if (fabs(cctt_speed_error) > cctt_max_speed)
	  cctt_speed_error_limited_abs = cctt_max_speed;
	else
	  cctt_speed_error_limited_abs = fabs(cctt_speed_error);

	/* Apply Brakes */
	if (cctt_speed_error < 0.0)
	{
	    if (cctt_max_speed > 0.0)
	      cctt_brake_pos_percent = 1.0 - 
		exp(-fabs(cctt_speed_error_limited_abs)/cctt_max_speed);
	    else
	      cctt_brake_pos_percent = 1.0;

	    tracked->commanded_throttle_pos = 0.0;
	}

	/* Apply Throttle */
	else
	{
            /* compute modified acceleration value */
	    cctt_accel_mod =  tracked->acceleration;

	    /* compute commanded throttle position rate */
	    cctt_commanded_throttle_pos_rate = 
	      tracked->params->throttle_forward_loop_gain*
	      (SGN(cctt_speed_error)*cctt_speed_error_limited_abs 
	       - tracked->params->throttle_accel_feedback_gain*cctt_accel_mod);

	    /* integrate throttle position */
	    tracked->commanded_throttle_pos += 
	      cctt_commanded_throttle_pos_rate*dt;

	    /* limit commanded throttle position */
	    if (tracked->commanded_throttle_pos > 1.0)
	      tracked->commanded_throttle_pos = 1.0;
	    else if (tracked->commanded_throttle_pos < 0.0)
	      tracked->commanded_throttle_pos = 0.0;

	    cctt_brake_pos_percent = 0.0;
	}
	    
	    /* compute throttle position */
	if (cctt_local_commanded_speed > 0.0)
	  tracked->throttle_pos += tracked->params->tau_throttle_inv*
	    dt*(tracked->commanded_throttle_pos - tracked->throttle_pos);
	else
	  tracked->throttle_pos = 0.0;

	/* limit throttle position */
	if (tracked->throttle_pos > 1.0)
	  tracked->throttle_pos = 1.0;
	else if (tracked->throttle_pos < 0.0)
	  tracked->throttle_pos = 0.0;

	/* CCTT Dynamics Section */

	/* Determine Driving Force */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_index[2] = cctt_get_index(tracked->throttle_pos, 0.0, 1.0,
				       tracked->params->n_throttle_positions);
	cctt_index[3] = cctt_get_index(cctt_slope, 0.0, PI_4, 
				       tracked->params->n_slopes);
	cctt_index[4] = cctt_get_index(speed, 0.0, cctt_max_speed, 
				       tracked->params->n_speeds);
	cctt_force_drive = TRACKED_CCTT_GRAVITY*
	  tracked_cctt_get_table_value(&tracked_cctt_table_force_drive, 
				       cctt_index);

	/* Scale drive force for reverse direction */
	if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE)
	  cctt_force_drive *= tracked->params->reverse_drive_factor;

        /* Determine Braking Force */
	if ((speed > TRACKED_CCTT_THRESHOLD) && 
	    (cctt_brake_pos_percent > TRACKED_CCTT_THRESHOLD))
	{
	    cctt_index[0] = cctt_vehicle_class;
	    cctt_index[1] = cctt_soil_type;
	    cctt_brake_linear_factor = tracked_cctt_get_table_value(
		&tracked_cctt_table_force_brake, cctt_index);
	    cctt_force_brake = pdb->mass_kg * cctt_brake_pos_percent * 
	      cctt_brake_linear_factor;
	    cctt_force_brake = tracked_cctt_get_table_value(
		&tracked_cctt_table_force_brake, cctt_index);
	}
	else
	  cctt_force_brake = 0.0;

	/* Determine Gravitational Force */
	cctt_weight_longitudinal = pdb->mass_kg*
	  TRACKED_CCTT_GRAVITY*cctt_slope_sin;
	cctt_weight_vertical     = pdb->mass_kg*
	  TRACKED_CCTT_GRAVITY*cctt_slope_cos;

	/* Ignore Gravity Going Downhill */
	if (((cctt_slope < 0.0) && 
	     (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)) ||
	    ((cctt_slope > 0.0) && 
	     (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE)))
	{
	    cctt_weight_longitudinal = 0.0;
	    cctt_weight_vertical     = pdb->mass_kg*TRACKED_CCTT_GRAVITY;
	}
	  
	/* Determine Friction Force */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_mu_eff_coeff = tracked_cctt_get_table_value(
	    &tracked_cctt_table_friction, cctt_index);
	cctt_force_friction = cctt_mu_eff_coeff*cctt_weight_vertical;

	if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD))
	{
	    cctt_force_friction = 0.0;
	    cctt_weight_longitudinal = 0.0;
	}

	/* Sum Forces */
	if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)
	  cctt_force = cctt_force_drive - cctt_weight_longitudinal - 
	    cctt_force_friction - cctt_force_brake;
	else if (cctt_local_gear == TRACKED_CCTT_GEAR_REVERSE)
	  cctt_force = -cctt_force_drive - cctt_weight_longitudinal + 
	    cctt_force_friction + cctt_force_brake;
	else
	  cctt_force = 0.0;

	/* Compute Acceleration */
	tracked->acceleration = cctt_force/pdb->mass_kg;

	/* Compute Speed */
	if (cctt_local_gear == TRACKED_CCTT_GEAR_FORWARD)
	  speed += tracked->acceleration*dt;
	else
	  speed -= tracked->acceleration*dt;
	speed = fabs(speed);

	if (NEARLY_ZERO(speed, TRACKED_CCTT_THRESHOLD))
	  speed = 0.0;
	else if (speed > cctt_max_speed)
	  speed = cctt_max_speed;

	/* Tangential sliding */
	ccttx_tangent_angle_sin = tracked->ccttx_sin_tangent_angle;
	ccttx_tangent_angle     = asin(ccttx_tangent_angle_sin);
	ccttx_tangent_angle_cos = cos(ccttx_tangent_angle);
	ccttx_tangent_angle_tan = ccttx_tangent_angle_sin /
	  ccttx_tangent_angle_cos;

	if (ccttx_tangent_angle_tan < 
	    -CCTTX_MU_COEFF_FACTOR*cctt_mu_eff_coeff)
	{
	    ccttx_tangent_slip_speed = CCTTX_TANGENT_SLIP_SPEED_CONSTANT;
	}
	else if (ccttx_tangent_angle_tan > 
		 CCTTX_MU_COEFF_FACTOR*cctt_mu_eff_coeff)
	{
	    ccttx_tangent_slip_speed = -CCTTX_TANGENT_SLIP_SPEED_CONSTANT;
	}
	else
	{
	    ccttx_tangent_slip_speed = 0.0;
	}
      }

    
    /*
     * Figure out what dust to generate
     */
    
    if (speed >= soil->dust_speeds_mps[2])
      dust = vehDustCloudLarge;
    else if (speed >= soil->dust_speeds_mps[1])
      dust = vehDustCloudMedium;
    else if (speed >= soil->dust_speeds_mps[0])
      dust = vehDustCloudSmall;
    else
      dust = vehDustCloudNone;

    /*
     *	Update the vehicle's state given its new velocity
     */


    /* Update position 
     */

    if (tracked->current_gear == HULLS_GEAR_REVERSE)
    {
	speed = -speed;
	dust = 0;
    }

    if (!still)
    {
	/* If we have a desired position, and the tick was kind of
	 * long.  Use the externally provided position instead.
	 */
	if ((tracked->control_state == TRACKED_EXTERNAL) &&
	    (dt > MAX_INTERNAL_TICK_SECONDS))
	{
	    VMAT2_VEC_COPY64(tracked->position, position);
	    position[CELL3D] = tracked->position[CELL3D];
	}
	else
	{
	    position[X] += speed * direction[X] * dt;
	    position[Y] += speed * direction[Y] * dt;
	    
	    if (tracked->params->mobility_model == TRK_CCTT_MOBILITY_MODEL_X)
	    {
		position[X] += ccttx_tangent_slip_speed*direction[Y]*dt;
		position[Y] -= ccttx_tangent_slip_speed*direction[X]*dt;
	    }
	}
    }

    /* If the vehicle is on the bridge, get its position and rotation
     * matrix from the bridge instead of the ground.
     */
    if (tracked->on_bridge)
    {
	float64  x_axis[3], y_axis[3], theta;

	tracked->bridge_collision_timeout = time_last_simulation_clock;

	ent_get_rotation_gcs(tracked->bridge_id, hull_to_world, NULL);

	/* We must now construct a rotation matrix for the vehicle that
	 * has the same z-axis as the bridge, and points in the direction
	 * of the vehicle.  We have x and y for this direction, we just
	 * need to find z.
	 *
	 * NOTE:  This method of calculating the rotation matrix of the
	 *        vehicle assumes that the roll angle of the bridge is
	 *        nearly zero (a pretty safe assumption for bridges).
	 */

	y_axis[X] = dx;
	y_axis[Y] = dy;
	y_axis[Z] = 0;
	vmat2_unit64(y_axis, y_axis);

	/* Find the angle between the outward normal of the bridge and
	 * (0, 0, 1).
	 */
	if(hull_to_world[Z][Z] > 1.0)
	  theta = 0.0;
	else
	  theta = acos(hull_to_world[Z][Z]);

	/* Find whether we're sloping up or down
	 */
	if(vmat2_dot_prod64(y_axis, hull_to_world[Z]) > 0)
	  theta = -theta;
	
	/* Compute the z-component of the vehicle's y-axis in world
	 * coordinates.
	 */
	y_axis[Z] = tan(theta);
	vmat3_unit64(y_axis, y_axis);

	hull_to_world[Y][X] = y_axis[X];
	hull_to_world[Y][Y] = y_axis[Y];
	hull_to_world[Y][Z] = y_axis[Z];

	/* Now that we have the y- and z-axes of the vehicle's rotation
	 * matrix (i.e. the 2nd and 3rd rows), we can get the x-axis
	 * by taking the cross product of the y- and z-axes.
	 */
	VMAT3E_CROSS_PROD(hull_to_world[Y][X], hull_to_world[Y][Y],
			  hull_to_world[Y][Z], hull_to_world[Z][X],
			  hull_to_world[Z][Y], hull_to_world[Z][Z],
			  x_axis);

	hull_to_world[X][X] = x_axis[X];
	hull_to_world[X][Y] = x_axis[Y];
	hull_to_world[X][Z] = x_axis[Z];

	/* The Z of the bridge is that at the center of the bridge.  The
	 * real Z will depend on where on the bridge the vehicle lies.
	 */

	/* First, find the slope of the bridge.
	 */
	if(bridge_dir[Z] != 1.0)
	  slope = bridge_dir[Z] / (1.0 - bridge_dir[Z] * bridge_dir[Z]);
	else
	  slope = 0.0;

	/* `dist_from_center' is the distance the center of the vehicle
	 * is AHEAD of the center of the bridge.  If this number is
	 * negative, then the vehicle hasn't reached the center of the
	 * bridge yet.
	 */

	/* First, find the magnitude of the distance.
	 */
	VMAT2_VEC_SUB(position, bridge_pos, temp_vec);

	dist_from_center = sqrt(temp_vec[X] * temp_vec[X] +
				temp_vec[Y] * temp_vec[Y]);

	/* Next, find out whether the vehicle has passed the center
	 * or not.
	 */
	if(VMAT2_DOT_PROD(bridge_dir, temp_vec) < 0.0)
	  dist_from_center = -dist_from_center;

	position[Z] = bridge_pos[Z] + slope * dist_from_center +
	  bridge_pdb->dimensions[Z];
	position[Z] += pdb->model_base_adjustment;

	new_soil = 0;
    }
    else
    {
	/* Generate the hull to world matrix.  Add a little to the Z seed,
	 * so that we don't fall through multi-level terrain.  The vehicle
	 * height is added to the Z seed because it is the largest value which
	 * is safe to add, since vehicles can't travel under overpasses which
	 * are lower than the top of the vehicle.  This should hopefully work
	 * when vehicles are traveling up or down steep multi-level
	 * microterrain slopes.
	 */
	/*    if (direction[Z] > 0.0)
	      position[Z] += direction[Z] * speed * dt;
	      */
	int32 index=1, tmp_mes_key_cell, saved_cell, cell;
        CTDB_MES_NODE_KEY saved, tmp_mes_key, tmp_mes_id;
	CTDB *tmp_mes_key_ctdb;
	float64 saved_z;

	cell = ctdb_get_cell_id(ctdb);

        saved_z = position[Z] += pdb->dimensions[Z];
        
        /* If the current node key is outside then check if there is another
         * node key in the list.  If so, then check if it is a valid mes key.
         * if it is try see if the current location is in the MES' footprint.
         * If so then try and place in that MES.
         */
        if ((mes_key == CTDB_INVALID_MES_NODE_KEY) &&
            tracked->node_look_fcn(tracked,1,&tmp_mes_key_cell,&tmp_mes_key) &&
	    tmp_mes_key_cell &&
            ctdb_mes_valid_node_key((tmp_mes_key_ctdb =
				     gcs_get_tdb(tmp_mes_key_cell, FALSE)),
				    tmp_mes_key) &&
            ctdb_mes_find_mes_from_xy(tmp_mes_key_ctdb,
				      position[X], position[Y],
                                      1, &tmp_mes_id) &&
            (CTDB_MES_NODE_KEY_TO_MES_ID(tmp_mes_key) ==
             CTDB_MES_NODE_KEY_TO_MES_ID(tmp_mes_id)) &&
	    (tmp_mes_key_cell == cell))
        {
	    mes_key_cell = tmp_mes_key_cell;
	    mes_key = tmp_mes_key;
        }

        /* First check the current MES key (that libentity has) to 
         * see if this was the proper enclosure.
         */
	saved_cell = mes_key_cell;
        saved = mes_key;
	ctdb_place_vehicle_ml(ctdb,
			      position[X], position[Y],
			      pdb->dimensions[Y], pdb->dimensions[X],
			      pdb->mass_kg, 
			      tracked->desired_dir[X],
			      tracked->desired_dir[Y],
			      &position[Z], hull_to_world, &new_soil,
			      &mes_key);

        /* If this was not the correct enclosure start looking ahead in 
         * the list to see if the next enclosure (...) is the correct one
         */
	if (saved != mes_key)
	{
	    saved_cell = ent_get_cell(vehicle_id);
	    saved = ent_get_mes_key(vehicle_id);
	    index = 0;
	    tracked_set_current(tracked, saved_cell, saved);
	}
	
        while (saved != mes_key || saved_cell != mes_key_cell)
        {
	    int32             next_mes_key_cell;
            CTDB_MES_NODE_KEY next_mes_key;
            /* If we have exhausted our list of choices break and 
             * keep the enclosure that ctdb_place_vehicle has placed 
             * us in.
             */
            if (!tracked->node_look_fcn(tracked,index++,&next_mes_key_cell,
					&next_mes_key))
              break;

            /* If the next choice for a mes_key is the one we had been 
             * placed in then keep the placement and stop looking.
             */
            if ((next_mes_key == mes_key) &&
		(next_mes_key_cell == mes_key_cell))
	      break;

	    saved_cell = mes_key_cell = next_mes_key_cell;
            saved = mes_key = next_mes_key;
	    position[Z] = saved_z;
            ctdb_place_vehicle_ml(gcs_get_tdb(mes_key_cell, FALSE),
				  position[X], position[Y],
                                  pdb->dimensions[Y], 
                                  pdb->dimensions[X],
                                  pdb->mass_kg, 
				  tracked->desired_dir[X],
				  tracked->desired_dir[Y],
                                  &position[Z], hull_to_world, &new_soil,
                                  &mes_key);
        }

	position[Z] += pdb->model_base_adjustment;

	/* Store tangential angle for enhanced CCTT mobility model */
	if (tracked->params->mobility_model == TRK_CCTT_MOBILITY_MODEL_X)
	  tracked->ccttx_sin_tangent_angle = hull_to_world[X][Z];
    }

    /* save our position to that we can see if it changed later..
       (perhaps from a teleport of a vehicle. */
    VMAT4_VEC_COPY64(position, tracked->saved_position);
    
    /* If the soil changed, look up the new soil */
    new_soil = CTDB_SIMNET_SOIL(new_soil);

    if (new_soil != tracked->params->soils[tracked->soil_latch].soil_type)
      tracked->soil_latch = tracked_find_soil(new_soil, tracked);

    /*
     * Set the entity parameters
     */

    velocity[X] = hull_to_world[Y][X] * speed;
    velocity[Y] = hull_to_world[Y][Y] * speed;
    velocity[Z] = hull_to_world[Y][Z] * speed;

    /* If appropriate,
     * add effect of wave action on hull.  Modifies position and hull_to_world */
    velocity[CELL3D] = position[CELL3D];

    add_wave_influence(vehicle_id, tracked, dt, cell, position, velocity,
		       hull_to_world, mes_key);

    ent_set_position_gcs(vehicle_id, position);
    DEBUG_TRACKED(("TRACKED position: (%.3lf, %.3lf, %.3lf, cell %d) (mes_id: %d encl_id: %d)\n",
		   position[X], position[Y], position[Z], (int32)position[CELL3D], 
		   CTDB_MES_NODE_KEY_TO_MES_ID(mes_key),
		   CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(mes_key)));

    ent_set_mes_key(vehicle_id, mes_key);
    ent_set_velocity_gcs(vehicle_id, velocity);
    ent_set_rotation_gcs(vehicle_id, hull_to_world, (int32)position[CELL3D]);

    /* save our position to that we can see if it changed later..
       (perhaps from a teleport of a vehicle. */
    tracked->saved_position[X] = position[X];
    tracked->saved_position[Y] = position[Y];
    tracked->saved_position[Z] = position[Z];

    old_appear = ent_get_appearance(vehicle_id);
    if ((old_appear & vehDustCloudMask) != dust)
      ent_set_appearance(vehicle_id,
			 (old_appear & ~vehDustCloudMask) | dust);

    /* Did we just run out of gas? */
    if (scmp_decrement(vehicle_id, munition_Fuel, 0,
		       dt * tracked_fuel_usage_rate(tracked->params,
						    fabs(speed)))
		       == 0.0)
    {
	tracked->state = TRACKED_STATE_NOGAS;
	cmpnt_available(vehicle_id, SM_TrackedHull, FALSE);
    }

    /* Reset the control inputs to zero, so we do not just wander off if
     * we stop getting commands for some period of time.
     */
#define NO_COMMAND_TIME 2000
    if ((time_last_simulation_clock - tracked->last_command_time) >
	NO_COMMAND_TIME)
    {
	float64 dir[2];

	tracked->control_state = TRACKED_EXPLICIT;
	vmat2e_unit64(hull_to_world[Y][X], hull_to_world[Y][Y], dir);

	tracked->desired_dir[X] = dir[X];
	tracked->desired_dir[Y] = dir[Y];
	tracked->desired_dir[CELL3D] = cell;
	
	tracked->speed = 0.0;
    }

    if (trkStats) 
      {
	float64 newDir[XYZC];
	ent_get_direction_gcs(vehicle_id, newDir);
	hulls_statsTickEnd(trkStats,         
			   vehicle_id,              
				     tracked->desired_gear,              
				     tracked->speed,                     
				     tracked->desired_dir,
				     position,
				     velocity,
				     newDir,
				     dt);
      }
}

static void flat_2D(
    float64  x,   
    float64  y,   
    float64 *xout,   
    float64 *yout)    
{
    float64 mag;

    mag = x * x + y * y;

    /* If mag is nearly one, then the vector is already normal */
    if ((mag > 0.9999) && (mag < 1.0001))
    {
	*xout = x;
	*yout = y;
    }
    else if (mag == 0.0)
    {
	*xout = 1.0;
	*yout = 0.0;
    }
    else
    {
	mag = 1.0 / fsqrt(mag);
	*xout = x * mag;
	*yout = y * mag;
    }
}


/* Add effect of wave action on hull.  Modifies position and hull_to_world
 * returns zero if no effect. */
static int32 add_wave_influence(
    int32 vehicle_id, 
    TRACKED_VARS *tracked, 
    float64 dt, 
    int32 cell,
    float64 position[XYZ], 
    float64 velocity[XYZ],
    float64 hull_to_world[XYZ][XYZ],
    CTDB_MES_NODE_KEY mes_key)
{
    float64  lposition[XYZC];
    float64  lvelocity[XYZ];
    float64  lhull_to_world[XYZ][XYZ];
    float64 wrot[3][3];
    float64 htow[3][3];
    int32 soil, new_soil;
    float64 temp_z;
    PHYSDB_DATA *pdb;
    float32 roll, pitch, yaw;
    ObjectType my_guise;
    CTDB *ctdb;
    ENV_INTERFACE ei;        /* environmental interface */
    CTDB_ELEV_DATA maxelev;
    static CTDB_ELEV_QUALIFIER qual=(CTDB_INCL_WATER |
                                CTDB_RETURN_MAX |
                                CTDB_INCL_MATERIAL);
    static int32 test_sign = -1;

    if (!tracked_wave_effect)
      return 0;

    my_guise = ent_get_guise(vehicle_id, 0);

    if (!tracked_have_rao(my_guise))
	return 0;

    if (!position)
    {
	ent_get_position_gcscs(vehicle_id, lposition, cell);
	ent_get_velocity_gcscs(vehicle_id, lvelocity, cell);
	ent_get_rotation_gcscs(vehicle_id, lhull_to_world, cell);
    }
    else
    {
        VMAT3_VEC_COPY64(position, lposition);
	VMAT3_VEC_COPY64(velocity, lvelocity);
	VMAT3_MAT_COPY64(hull_to_world, lhull_to_world);
    }
    lposition[CELL3D] = cell;
    /*
     * We don't want wave effects if we're driving on solid ground.
     *
     * The following call asks CTDB for the elevation of the water
     * surface, if there is one at the given location.
     */
    ctdb = ent_get_ctdb(vehicle_id);

    soil = ctdb_lookup_soil(ctdb, lposition[X], lposition[Y]);
    if( CTDB_SIMNET_SOIL(soil) != 4 && CTDB_SIMNET_SOIL(soil) != 5 &&
	CTDB_SIMNET_SOIL(soil) != 9)
      return 0;

    vmat3_rotation64_to_zdown_angles32(lhull_to_world, &yaw, &pitch, &roll);
    lposition[Z] = 0.;
    pitch        = 0.;
    roll         = 0.;

    /* tracked_run_rao() sets absolute pitch, roll, and position[Z] */

    tracked_run_rao(vehicle_id, tracked, my_guise, dt,
		    &roll, &pitch, lposition, &lhull_to_world[Y][0], lvelocity);

    /*
     * Calculate the effect of current on our position.  The main tick
     * code has calculated movement across the surface.  Here, we add
     * the effect of movement of that surface.  This is analogous to 
     * headwind/tailwind on aircraft.  Note that this is distinct from
     * movement due to wave (surge/heave/sway).
     */
    if (!first_tick) /* dt provided on first tick is bogus */
    {
	bzero(&ei, sizeof(ENV_INTERFACE));    
	ei.u.sea_current_velocity.input_vector[X] = lposition[X];
	ei.u.sea_current_velocity.input_vector[Y] = lposition[Y];
	ei.u.sea_current_velocity.input_vector[Z] = lposition[Z];
	ei.u.sea_current_velocity.input_vector[CELL3D] = cell;
	env_get(ENV_SEA_CURRENT_VELOCITY, &ei);
	lposition[X] += ei.u.sea_current_velocity.output_vector[X] * dt;
	lposition[Y] += ei.u.sea_current_velocity.output_vector[Y] * dt;
    }

    vmat3_zdown_angles32_to_rotation64(yaw, pitch, roll, lhull_to_world);

    if (!position)
    {
	lposition[CELL3D] = cell;
        ent_set_position_gcs(vehicle_id, lposition);
	ent_set_rotation_gcs(vehicle_id, lhull_to_world, cell);
	return 1;
    }

    VMAT3_VEC_COPY64(lposition, position);
    if (hull_to_world)
      VMAT3_MAT_COPY64(lhull_to_world, hull_to_world);

    return 1;
}


static void skew(int32 vehicle_id)
{
    float64 skew[3][3];
    float64 htow[3][3];
    float64 angle;
    int32 cell;
    
    ent_get_rotation_gcs(vehicle_id, htow, &cell);

    /* -5 .. -3, 3 .. 5 degrees around Y */
    angle = rnd_uniform_dist() * 4.0;
    if (angle < 2.0)
      angle -= 5.0;
    else
      angle += 1.0;
    angle = DEG_TO_RAD(angle);

    vmat3_primary_rotation64(Y, sin(angle), cos(angle), skew);
    vmat3_mat_mat_mul64(htow, skew, htow);

    /* -8 .. -6, 6 .. 8 degrees around X */
    angle = rnd_uniform_dist() * 4.0;
    if (angle < 2.0)
      angle -= 8.0;
    else
      angle += 6.0;
    angle = DEG_TO_RAD(angle);

    vmat3_primary_rotation64(X, sin(angle), cos(angle), skew);
    vmat3_mat_mat_mul64(htow, skew, htow);

    ent_set_rotation_gcs(vehicle_id, htow, cell);
}

static void get_soil(
    int32         vehicle_id,   
    TRACKED_VARS *tracked,   
    CTDB         *ctdb)    
{
    uint32 soil;
    double position[XYZC];

    float64 adj_z; 
    ent_get_position_gcs(vehicle_id, position);

    adj_z = position[Z] + 1.0;/* safety margin for multi-level */
    
    soil = CTDB_SIMNET_SOIL(ctdb_lookup_soil_ml(ctdb, position[X],
						position[Y], adj_z));

    tracked->soil_latch = tracked_find_soil(soil, tracked);
}

int32 tracked_find_soil(
    uint32        soil,   
    TRACKED_VARS *tracked)    
{
    int32 i;
    int32 zero=0;

    /* Find this soil type (or a zero soil type, by default) */
    /* $$$ This is a temporary change to allow an interim solution for
     * databases with bathymetry to work.  Basically, we're redefining
     * soil type 9.  We now want it to behave like soil type 4.  We are
     * putting the change here instead of adding soil type 9 to all of
     * the vehicle parameter files.
     */
    if(soil == 9)
      soil = 4;

    for (i=0;i<tracked->params->n_soils;i++)
    {
	if (tracked->params->soils[i].soil_type == soil)
	  return i;

	if (!tracked->params->soils[i].soil_type)
	  zero = i;
    }
    return zero;
}

static void tick_mini_driver(
    int32         vehicle_id,   
    TRACKED_VARS *tracked,   
    float64       dt)    
{
    float64 position[XYZC];
    float64 direction[XYZC];
    float64 dx, dy;

    ent_get_position_gcs(vehicle_id, position);
    ent_get_direction_gcs(vehicle_id, direction);


    /* make sure that tracked->position is with respect to the
     * vehicles current cell!  Convert it in place...
     */

    if (position[CELL3D] != tracked->position[CELL3D])
    {
	gcs_gcs3_to_gcscs64((int32)tracked->position[CELL3D],
			    tracked->position,
			    (int32)position[CELL3D], 
			    position);
	tracked->position[CELL3D] = position[CELL3D];
    }

    /*
     * These are stubs.  Be sure to replace with better algorithms.
     */

    tracked->max_accel = tracked->max_turn = 0.0;

    switch (tracked->control_state)
    {
      case TRACKED_POSITION_DIRECTION:
	dx = tracked->position[X] - position[X];
	dy = tracked->position[Y] - position[Y];

	if (NEARLY_ZERO(dx,1.0) && NEARLY_ZERO(dy,1.0))
	{
	    tracked->speed = 0.0;
	    tracked->desired_dir[X] = tracked->direction[X];
	    tracked->desired_dir[Y] = tracked->direction[Y];
	    tracked->desired_dir[CELL3D] = tracked->direction[CELL3D];
	}
	else
	{
	    tracked->desired_dir[CELL3D] = position[CELL3D];

	    /* Face the point */
	    if (dx*direction[X] + dy*direction[Y] > 0)
	    {
		tracked->desired_gear = HULLS_GEAR_FORWARD;
		tracked->desired_dir[X] = dx;
		tracked->desired_dir[Y] = dy;
	    }
	    else
	    {
		tracked->desired_gear = HULLS_GEAR_REVERSE;
		tracked->desired_dir[X] = -dx;
		tracked->desired_dir[Y] = -dy;
	    }

	    /* Approach at a speed such that we get halfway to the point
	     * on our next tick.  (There is a much better algorithm
	     * which uses acceleration/deceleration information, but
	     * this is just a stub.)
	     */
	    tracked->speed = 0.5 * fsqrt(dx*dx + dy*dy) / dt;
	}
	break;
      case TRACKED_GOAL_CORRIDOR:
	/* This stub is worse that the previous one.  It just ignores the
	 * corridor altogether.
	 */
	tracked->desired_dir[X] = tracked->position[X] - position[X];
	tracked->desired_dir[Y] = tracked->position[Y] - position[Y];
	tracked->desired_dir[CELL3D] = position[CELL3D];
	tracked->desired_gear = HULLS_GEAR_FORWARD;
	tracked->speed = tracked->approach_speed;
	break;
      case TRACKED_TARGET_ID:
	/* Where is this guy? */
	ent_get_position_gcscs(tracked->target_id, tracked->position,
			       (int32)position[CELL3D]);

	tracked->position[CELL3D] = position[CELL3D];

      case TRACKED_TARGET_POSITION:
	/* Ramming speed!
	 */
	tracked->desired_dir[X] = tracked->position[X] - position[X];
	tracked->desired_dir[Y] = tracked->position[Y] - position[Y];
	tracked->desired_dir[CELL3D] = position[CELL3D];

	tracked->desired_gear = HULLS_GEAR_FORWARD;
	tracked->speed = 1000.0; /* A really big number meaning MAXIMUM */
	break;
    }
}


float64 tracked_compute_eta(
    int32         vehicle_id,   
    TRACKED_VARS *tracked,   
    float64      *target,   
    int32         cell)    
{
    struct tracked_soils *soil;
    float64 position[XYZ];
    float64 dx, dy, dist;

    ent_get_position_gcscs(vehicle_id, position, cell);

    /* Make sure we know what soil type we're using (only happens
     * before first tick).
     */
    if (tracked->soil_latch == -1)
      soil = tracked->params->soils; /* Use 0 by default */
    else
      soil = tracked->params->soils+tracked->soil_latch;

    /* This algorithm has much room for improvement:
     * -> consider acceleration to reach max speed
     * -> consider soil types in between
     * -> consider grade in between
     */

    dx = target[X] - position[X];
    dy = target[Y] - position[Y];
    dist = fsqrt(dx*dx + dy*dy);

    return (dist / soil->max_speeds_mps[TRACKED_MAX_FORWARD]);
}

/* ARGSUSED */
void tracked_collision(
    int32                        vehicle_id,   
    COLLISION_NOTIFICATION_DATA *data,   
    ADDRESS                      user_data)    
{
    TRACKED_VARS *tracked = (TRACKED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  tracked_user_data_handle);

    float64 position[XYZC];
    int32 other_id;
    int32 coll_type;
    float64 my_pos[XYZC];
    float64 velocity[XYZC];

    float64 dp[XYZ];
    float64 other_velocity[XYZ];

    float64 other_mass;
    float64 speed;
    float64 d_momentum;
    PHYSDB_DATA *pdb;

    int32   cell = ent_get_cell(vehicle_id);
    

    if (!tracked)		/* Passive error detection */
      return;

    /* Copy some useful data out of the collision data */
    coll_type = data->coll_type;
    other_id = data->other_id;
    other_mass = data->other_mass;

    /* this is equivelent to position := data->position except that
     * it insures everything is with repect to the entities current
     * cell
     */
    gcs_gcs3_to_gcscs64(data->cell, data->position,
			cell, position);

    position[CELL3D] = cell;
    
    /* ditto for velocity.. */
    gcs_vec_to_vec64(data->cell, data->other_velocity,
		     cell, other_velocity);


    /* Note:
     *
     * Rather than doing a real Newtonian model (which would be pretty
     * easy) to figure out our new velocity, we instead do this stuff.
     * This is because other SIMNET simulators don't use a real model,
     * so if we decided to just plow forward, we would probably penetrate
     * the other party in the collision.  That would be bad.
     */

    /* If we aren't moving, allow ourselves to be nudged */
    if (ent_get_stationary(vehicle_id))
    {
	pdb = ent_get_physdb(vehicle_id);

	/* Assume the collision is perfectly inelastic, so our
	 * combined masses will move at some new velocity such that:
	 *      momentum == (m1 + m2) * new_velocity
	 * so,  new_velocity = (momentum) / (m1 + m2)
	 */
	
	/* Compute his momentum */
	d_momentum = other_mass / (other_mass + pdb->mass_kg);
	VMAT3_SCAL_VEC_MUL(d_momentum, other_velocity, velocity);

	/* Rather than doing all the friction calculations to figure
	 * out how long we would slide at this rate, just apply a simple
	 * heuristic.
	 */
	VMAT3_SCAL_VEC_MUL(SLIDE_TIME, velocity, dp);

	ent_get_position_gcs(vehicle_id, my_pos);

	VMAT3_VEC_ADD(my_pos, dp, my_pos);
	ent_set_position_gcs(vehicle_id, my_pos);
	
	/* save our position to that we can see if it changed later..
	   (perhaps from a teleport of a vehicle. */

	VMAT4_VEC_COPY64(position, tracked->saved_position);
    }
    else
    {
	ent_get_velocity_gcs(vehicle_id, velocity);

	/* NOTE: This is broken under SIMNET.  If the other guy rear ends
	 * us, and detects the collision first, he will immediately stop
	 * moving.  When we then get the collision packet, it looks like
	 * he isn't moving so we assume we collided with him.  The net
	 * result is that we both recoil from the impact.
	 */

	/* Ignore the collision unless we and the other party are moving
	 * in opposite directions or we were moving faster (implying
	 * that *we* rear-ended him).
	 */
	if ((VMAT2_DOT_PROD(velocity, other_velocity) <= 0.0) ||
	    (ent_get_speed_squared(vehicle_id) >
	     VMAT3_DOT_PROD(other_velocity, other_velocity)))
	{
	    /* Recoil at 1 mps (this shouldn't look to bad, and will
	     * give the driver a moment to react).
	     */
	    speed = -1.0 / ent_get_speed(vehicle_id);
	    VMAT3_SCAL_VEC_MUL(speed, velocity, velocity);

	    ent_set_velocity_gcs(vehicle_id, velocity);
	}
    }
}

void tracked_bridge_collision(
    int32    vehicle_id,   
    int32    bridge_id,   
    ADDRESS  user_data)    
{
    float64                      bridge_pos[XYZC], my_pos[XYZC];
    float64                      dist1, dist2, dx, dy;
    TRACKED_VARS *tracked = (TRACKED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  tracked_user_data_handle);

    if (!tracked)		/* Passive error detection */
      return;

    /* Check to see if this bridge is closer than the current `bridge_id'.
     */
    if (tracked->bridge_id && (tracked->bridge_id != bridge_id))
    {
	ent_get_position_gcs(vehicle_id, my_pos);
	ent_get_position_gcscs(bridge_id, bridge_pos, (int32)my_pos[CELL3D]);

	dx = bridge_pos[X] - my_pos[X];
	dy = bridge_pos[Y] - my_pos[Y];
	dist1 = dx*dx + dy*dy;

	ent_get_position_gcscs(tracked->bridge_id, bridge_pos,
			       (int32)my_pos[CELL3D]);

	dx = bridge_pos[X] - my_pos[X];
	dy = bridge_pos[Y] - my_pos[Y];
	dist2 = dx*dx + dy*dy;

	if(dist1 > dist2)
	  return;
    }

    tracked->bridge_id = bridge_id;
    tracked->bridge_collision_timeout = time_last_simulation_clock + 5000;
}

static void tracked_request_smoke(
    int32  vehicle_id)    
{
    float64			velocity[XYZC];
    ENV_EFFECT_INTERFACE	ei;
    
    ent_get_velocity_gcs(vehicle_id, velocity);
    
    if ((velocity[X] != 0.0) || 
	(velocity[Y] != 0.0) ||
	(velocity[Z] != 0.0))
    {
	sched_deferred_fncl(tracked_request_smoke, 500, vehicle_id,
			    A_INT, vehicle_id, A_END);
	return;
    }

    /* Request the smoke type for this vehicle */
#if 0
    ei.u.smoke.source = ent_get_guise(vehicle_id, 0);
#else
    ei.u.smoke.source = environmental_Smoke_DFR_Medium;
#endif

    ent_get_position_gcs(vehicle_id, ei.u.smoke.location);

    ei.u.smoke.num_sources = 1;
	ei.u.smoke.source_type = ENV_SOURCE_POINT_DETONATION; 

    env_request_effect(ENV_SMOKE_EFFECT, &ei);
}

void tracked_damage(
    int32    vehicle_id,   
    uint32   new_kill_level,   
    uint32   old_kill_level,   
    ADDRESS  user_data)    
{
    char  msg[100];
    int32 damaged;
    int32 is_cat_kill;
    uint32 mask = ENT_CAT_KILL_MASK | ENT_MOB_KILL_MASK;

    TRACKED_VARS *tracked = (TRACKED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  tracked_user_data_handle);

    if (!tracked || !tracked->cells_initted)	/* Passive error detection */
      return;

    /* if this isn't a transition in the cat or mob bits return */
    if(!((new_kill_level ^ old_kill_level) & mask))
      return;

    /* Set the damage field bassed on if the fields were turned */
    /* on (damage is true) or turned off (damage is false).     */
    /* Both cat and fire must be off for a repair.              */
    if(new_kill_level & mask)
      damaged = TRUE;
    else
      damaged = FALSE;

    /* Create smoke if this is a new cat_kill */
    if((new_kill_level & ENT_CAT_KILL_MASK) &&
       !(old_kill_level & ENT_CAT_KILL_MASK))
      tracked_request_smoke(vehicle_id); 


    /* If this is a cat kill set the is_cat_kill flag */
    if((new_kill_level & ENT_CAT_KILL_MASK) ||
       (old_kill_level & ENT_CAT_KILL_MASK))
      is_cat_kill = TRUE;
    else
      is_cat_kill = FALSE;

    if (damaged)
      ent_set_appearance_bits(vehicle_id, vehMobilityDisabled);
    else
      ent_unset_appearance_bits(vehicle_id, vehMobilityDisabled);      

    if (is_cat_kill)
    {
	if (damaged)
	{
/*	    tracked_request_smoke(vehicle_id); */

	    ent_set_appearance_bits(vehicle_id,
				    (vehDestroyed | vehSmokePlume | vehFlaming));

	    sched_simtime_fncl(ent_unset_appearance_bits,
			       time_last_simulation_clock + (15 * 60 * 1000), 
			       vehicle_id,
			       A_INT, vehicle_id,
			       A_INT, vehFlaming,
			       A_END);
	    sched_simtime_fncl(ent_unset_appearance_bits, 
			       time_last_simulation_clock + (30 * 60 * 1000),
			       vehicle_id,
			       A_INT, vehicle_id,
			       A_INT, vehSmokePlume,
			       A_END);

	    if (!((old_kill_level & new_kill_level) & ENT_CAT_KILL_MASK))
	    {
		/* Notify the Operator this is a catastrophic kill */
		sprintf(msg, "Vehicle %d catastrophic kill",
			vehicle_id);
		alert_user(vehicle_id, 0, ALERT_DEST_VEH_IIGUI, msg);
		
		/* When a vehicle is destroyed a secondary explosion occurs */
		arty_send_detonation_PDU(
		    munition_US_Mk82,
		    munition_US_M904,
		    (int32)tracked->saved_position[CELL3D],
		    tracked->saved_position,
		    0);
	    }

	}
	else
	  ent_unset_appearance_bits(vehicle_id, 
				    vehDestroyed | vehSmokePlume | vehFlaming);
    }

    if (damaged)
    {
	/* Nothing to do if the vehicle is already dead or dying */
	if ((tracked->state == TRACKED_STATE_DEAD) ||
	    (tracked->state == TRACKED_STATE_DYING))
	  return;
	else
	  tracked->state = TRACKED_STATE_DYING;
    }
    else
    {
	if (tracked->state == TRACKED_STATE_DEAD)
	{
	    tracked->state = TRACKED_STATE_HEALTHY;
	    cmpnt_available(vehicle_id, SM_TrackedHull, TRUE);
	}
    }
}

/*
 * cctt_get_index()
 * 
 * cctt_get_index computes a discreet value from a continous range, 
 * given the value, minimum, maximum, and level of discretization. 
 * The maximum value must be greater than the minimum.
 *
 */
static int32 cctt_get_index(
    float64  value,   
    float64  min,   
    float64  max,   
    int32    n_values)    
                                         /* input value */
                                         /* minimum value */
                                         /* maximum value */
                                         /* number of values */
{
    int32              index;            /* discritization */

    /* error checking */
    if (min == max)
      return 0;

    /* offset value */
    value -= min;

    /* round to nearest integer */
    index = (int32) ((float64) n_values)*
      ((float64) value)/((float64) (max-min));

    /* check index */
    if (index < 0) 
      return 0;
    if (index >= n_values)
      return n_values-1;

    return index;
}

static int32 trk_wet_weather_on(
    TRACKED_VARS        *tracked,
    float64              position[XYZC])
{
    CTDB_ABSTRACT_DATA *data;
    ENV_INTERFACE       ei;
    
    bzero(&ei, sizeof(ENV_INTERFACE));
    ei.u.generic_visual_check.input_viewer[X] = position[X];
    ei.u.generic_visual_check.input_viewer[Y] = position[Y];
    ei.u.generic_visual_check.input_viewer[Z] = position[Z];
    ei.u.generic_visual_check.input_viewer[CELL3D] = position[CELL3D];
    
    if(time_last_simulation_clock - tracked->last_check_time >
       CHECK_PERIOD)
    {
	tracked->last_check_time = time_last_simulation_clock;
	
	tracked->within_hydro_subsurface =
	  ctdb_point_within_abstract((CTDB*)gcs_get_tdb((int32)position[CELL3D],
							FALSE),
				     CTDB_ABSTRACT_HYDRO_SUBSURFACE,
				     position[X], position[Y], &data) &&
	  data->hydro_subsurface.wetness;
    }
    
    if ((env_get(ENV_PRECIPITATION_RATE, &ei) != -1) &&
	(env_get(ENV_PRECIPITATION_TYPE, &ei) != -1))
      if (((ei.u.precipitation_rate.output_value > 0.0) && 
	   (ei.u.precipitation_type.output_type > 0)) ||
	  tracked->within_hydro_subsurface)
	return(TRUE);
      else
	return(FALSE);
    
    return(FALSE);
}

static int32 trk_in_trouble_slope(
    float64               soil_slope,
    TRACKED_VARS         *tracked,
    float64               position[XYZC],
    struct tracked_soils *soil)
{
    if(trk_wet_weather_on(tracked, position) && 
       (soil_slope > soil->max_wet_slope))
      return TRUE;
  
    if(!trk_wet_weather_on(tracked, position) && 
       (soil_slope > soil->max_dry_slope))
      return TRUE;
  
    return FALSE;
}
	
static int32 trk_simnet_to_cctt_soil_mapping(
    TRACKED_VARS  *tracked,
    float64        position[XYZC],
    int32          soil_type)
{
    int32 i;
  
    if(trk_wet_weather_on(tracked, position))
      /* wet weather */
    {
	for (i=0; i< TRACKED_MAX_SOIL_TYPE; i++)
	{
	    if (tracked_soil_tables[1].soil_table[i].simnet_soil_type
		== soil_type)
	      return(tracked_soil_tables[1].soil_table[i].cctt_soil_type);
	}
	return(0);
    }
    else
    {
	for (i=0; i< TRACKED_MAX_SOIL_TYPE; i++)
	{
	    if (tracked_soil_tables[0].soil_table[i].simnet_soil_type
		== soil_type)
	      return(tracked_soil_tables[0].soil_table[i].cctt_soil_type);
	}
	return(0);
    }
    return(0);  
}

/*=======================================================================
turn on statistics gathering, called from parser */

void tracked_statsOn(void)
{
  if (!trkStats) hulls_statsInit(&trkStats);
  else hulls_statsOn(trkStats);
  tracked_statsStatus();
}

/*========================================================================
turn off statistics gathering, called from parser */

void tracked_statsOff(void)
{
  if (trkStats) hulls_statsOff(trkStats);
  tracked_statsStatus();
}

/*========================================================================
zero out statistics, called from parser */

void tracked_statsReset(void)
{
  if (trkStats) 
    {
      hulls_statsReset(trkStats);
      tracked_statsStatus();
    }
  else printf("No tracked stats memory allocated, use \"On\" command\n");
  
}

/*========================================================================
print out the current statistics, called from parser */

void tracked_statsPrint(void)
{
  if (trkStats) 
    {
      printf("Tracked stats:\n");
      hulls_statsPrint(trkStats);
    }
  else printf("No tracked stats data, use \"on\" command\n");
}

/*========================================================================
printout the current settings for statistics gathering */

void tracked_statsStatus(void)
{
  printf("Tracked stats ");
  hulls_statsStatus(trkStats);
  return;
}

/*=======================================================================
delete statistics and deallocate memory */

void tracked_statsQuit(void)
{
  if (trkStats) hulls_statsDestroy(&trkStats);
  tracked_statsStatus();
}

