/****************************************************************************
 *   File: whl_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: wtaylor                                                     *
 *      $Revision$                                                    *
 *       Remarks:                                                            *
 *                                                                           *
 ****************************************************************************/

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

#define WHL_LOWER_TICK_BOUND   0.08
#define WHL_UPPER_TICK_BOUND   0.12
#define WHL_MIN_RES_LEVEL      0
#define WHL_MAX_RES_LEVEL      2
#define WHL_HALF_PI            1.570796327
#define WHL_HALF_PI_INVERSE    0.6366197724
#define WHL_TO_DEG            57.29577951
#define WHL_G_CONST            9.81

#define NEARLY_ZERO(x,err) (((x) > -err) && ((x) < err))
#define CHECK_PERIOD 10000           /* miliseconds */
#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 */
#define PI_4 0.7853990675


/* PRIVATE FUNCTIONS
------------------------------------------------------------*/

static int32 whl_in_trouble_slope(
    float64 soil_slope,
    WHEELED_VARS *wheeled,
    float64       position[XYZC],
    struct wheeled_soils *soil);

static int32 whl_wet_weather_on(
    WHEELED_VARS *wheeled,
    float64      position[XYZC]);

static int32 whl_simnet_to_cctt_soil_mapping(
    WHEELED_VARS  *wheeled,
    float64       position[XYZC],
    int32         soil_type);

static float64 whl_get_veh_max_speed(
    WHEELED_VARS  *wheeled,
    float64       position[XYZC],
    int32 cctt_soil);

static void skew(
    int32  vehicle_id);    

static void get_soil(
    WHEELED_VARS *wheeled,   
    float64       position[XYZC]);    

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

static void mini_driver(
    WHEELED_VARS *wheeled,   
    float64       desired_speed,   
    float64       speed,   
    float64       delta_t,   
    float64      *desired_direction,   
    float64      *direction,   
    int32        *current_gear,   
    float64      *desired_wheel_angle,   
    float64      *desired_acceleration,   
    float64      *desired_brake,   
    int32        *desired_gear);    

#if 0
static void tick_mini_driver(
    int32         vehicle_id,   
    WHEELED_VARS *wheeled,   
    float64       dt);    
#endif

static void dynamics(
    WHEELED_VARS *wheeled,   
    PHYSDB_DATA  *pdb,   
    float64       max_speed,   
    int32         soil_type,   
    float64       desired_speed,   
    float64       desired_wheel_angle,   
    float64       desired_acceleration,   
    float64       desired_brake,   
    int32         desired_gear,   
    float64       delta_t,   
    float64      *position,   
    float64      *turn_angle,   
    float64      *speed,   
    int32        *current_gear);    
    
static int32 cctt_get_index(
    float64            value,            /* input value */
    float64            min,              /* minimum value */
    float64            max,              /* maximum value */
    int32              n_values);        /* number of values */

/* hook for wheeled movement statistics */
static HULLS_STATS *wheeledStats = NULL;

/* WHEELED TICK
 *  
 * First computes resolution level, then gets vehicle inputs from a
 * mini driver and then ticks the dynamics according to that
 * resolution under those inputs.
 */
void wheeled_tick(
    int32    vehicle_id,   
    uint32   interval_since_last_tick,   
    ADDRESS  user_data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);

    float64 position[XYZC];
    float64 velocity[XYZC];
    float64 direction[XYZC];
    float64 hull_to_world[XYZ][XYZ];
    float64 bridge_dir[XYZ], bridge_pos[XYZ];
    float64 temp_vec[XY];
    
    float64 dx, dy, odx, ody;	/* orientation of the vehicle */

    int32 still, right_direction;
    float64 speed, desired_speed, desired_acceleration, desired_direction[2],
      desired_wheel_angle, desired_brake;
    int32 desired_gear, current_gear;
    struct wheeled_soils *soil;

    CTDB_MES_NODE_KEY mes_key;
    int32 mes_key_cell;
    int32 new_soil;
    float64 max_mps, dz;
    float64 turn_angle=0.0;
    float64 dt;
    float64 cross, dot;
    float64 slope;
    float64 dist_from_center;
    PHYSDB_DATA *pdb, *bridge_pdb;
    uint32 old_appear, dust;
    int32 soil_type;
    CTDB *ctdb = ent_get_ctdb(vehicle_id);
    int32 num_lanes, i;
    float64 dist, temp_dist;
    float64 slope_sin, soil_slope, veh_max_speed;
    int32  cctt_soil;
    int32   cell = ent_get_cell(vehicle_id);

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

    if (wheeled->being_moved)   /* We're being towed, don't bother */
      return;
    
    /* Nothing to do if the vehicle is dead.
       Still may want to compute dynamics, though, if, for instance, 
       someone else collides with dead vehicle. */

    if (wheeled->state == WHEELED_STATE_DEAD)
    {
	/* may want to check for a collision here.  how to do that? */
	return;
    }
    
    dt = interval_since_last_tick / 1000.0;
    
    /* Careful about stopping the simulation */
    if (dt == 0.0) 
      return;
    
    /* 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(wheeledStats) hulls_statsTickStart(wheeledStats,
					  vehicle_id,
					  wheeled->speed,
					  position,
					  direction,
					  dt);

    /* get previous turn angle */
    turn_angle = NS_ATAN2(direction[Y],direction[X]);
    
    /* get previous speed 
       speed = sqrt(velocity[X]*velocity[X] + velocity[Y]*velocity[Y] + 
       velocity[Z]*velocity[Z]);*/
    
    /* Cache whether we are not moving */
    still = ((velocity[X] == 0.0) && (velocity[Y] == 0.0));

    /* make sure that wheeled->desired_dir is with respect to
     * the entities current cell.
     */
    if (cell != wheeled->desired_dir[CELL3D])
    {
	/* perform the conversion in place.. */
	gcs_vec_to_vec64((int32)wheeled->desired_dir[CELL3D],
			 wheeled->desired_dir,
			 (int32)cell,
			 wheeled->desired_dir);
	wheeled->desired_dir[CELL3D] = cell;
    }
    
    if (!wheeled->cells_initted)
    {
	/* this is a hack.  since we cannot find out the vehicles
	 * current direction in wheeled_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..  
	 */
	wheeled->cells_initted = TRUE;
	VMAT4_VEC_COPY64(direction, wheeled->desired_dir);
	wheeled->node_keys.list_cell[0] = ent_get_cell(vehicle_id);
    }

    if (!wheeled->node_look_fcn(wheeled,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], wheeled->saved_position[X]))) ||
	(!(CLOSE_ENOUGH(position[Y], wheeled->saved_position[Y]))) ||
	(position[CELL3D] != wheeled->saved_position[CELL3D]))
    {
	pdb = ent_get_physdb(vehicle_id);
	ctdb_place_vehicle_ml(ctdb, position[X], position[Y],
			      pdb->dimensions[Y], pdb->dimensions[X], 
			      pdb->mass_kg,
			      wheeled->desired_dir[X], wheeled->desired_dir[Y],
			      position+Z, hull_to_world, &new_soil,
			      &mes_key); 
	
	ent_set_rotation_gcs(vehicle_id, hull_to_world, cell);
	
	/* save our position to that we can see if it changed later..
	 * (perhaps from a teleport of a vehicle. 
	 */
	VMAT4_VEC_COPY64(position, wheeled->saved_position);
	
	/* 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);
	wheeled_clear_node_key_list(wheeled,mes_key_cell,mes_key);
	wheeled_set_current(wheeled,mes_key_cell,mes_key);
    }
    
    /* 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]*wheeled->desired_dir[Y] - 
      direction[Y]*wheeled->desired_dir[X];
    dot = direction[X]*wheeled->desired_dir[X] + 
      direction[Y]*wheeled->desired_dir[Y];

    right_direction = NEARLY_ZERO(cross,0.001) && (dot > 0.0);
    
    /* Grab the desired speed. */
    desired_speed = wheeled->speed;

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

	    desired_speed = 0.0; /* this is not right */
	}
    }

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

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

    /* Are we stuck? */
    if (wheeled->state == WHEELED_STATE_STUCK)
    {
	/* check to make sure everything is in the same cell. */
	if (cell != wheeled->stuck_position[CELL3D])
	{
	    /* convert it in place. */
	    gcs_gcs2_to_gcscs64((int32)wheeled->stuck_position[CELL3D],
				wheeled->stuck_position,
				cell, 
				wheeled->stuck_position);
	    wheeled->stuck_position[CELL3D] = cell;
	}
	
	/* Have we been towed? */
	dx = position[X] - wheeled->stuck_position[X];
	dy = position[Y] - wheeled->stuck_position[Y];

 	if (!NEARLY_ZERO(dx,0.001) || !NEARLY_ZERO(dy,0.001))
	{
	    wheeled->state = WHEELED_STATE_HEALTHY;
	    cmpnt_available(vehicle_id, SM_WheeledHull, TRUE);
	    /* Note that we don't know what we're on now. */
	    wheeled->soil_latch = -1;
	}
	else
	  return;
    }

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

	    wheeled->state = WHEELED_STATE_DEAD;
	    cmpnt_available(vehicle_id, SM_WheeledHull, 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 &&
	(wheeled->soil_latch != -1))
      return;

    pdb = ent_get_physdb(vehicle_id);

    /* Figure out if we are on a bridge or not.
     */

    if(wheeled->bridge_id)
    {
	float64       bridge_poly[4][2];
	float64       bridge_parallel[2], bridge_perp[2];

	ent_get_position_gcscs(wheeled->bridge_id, bridge_pos, 
			       (int32)position[CELL3D]);
	ent_get_direction_gcscs(wheeled->bridge_id, bridge_dir,
				(int32)position[CELL3D]);

	/* Compute the polygon which surrounds the bridge.
	 */
	bridge_pdb = ent_get_physdb(wheeled->bridge_id);
	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, bridge_parallel,
			   bridge_parallel);
	VMAT2_SCAL_VEC_MUL(bridge_pdb->dimensions[X] * 0.5, 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?
	 */
	wheeled->on_bridge = geo_is_point_in_convex_polygon(position,
							    bridge_poly, 4)
	  == GEO_PT_INSIDE;

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

	    wheeled->soil_latch = 0;
	    desired_speed = NS_MIN(desired_speed, speeds[2].real);
	}

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

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

#if 0
    /* Do we want to change gear? */
    if (wheeled->current_gear != wheeled->desired_gear)
    {
	if (speed < 0.1)
	  wheeled->current_gear = wheeled->desired_gear;
	else /* Don't allow gear change until we stop */
	  desired_speed = 0.0;
    } 
#endif

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

    /* Get the parameters for the soil we're on. */
    soil = wheeled->params->soils+wheeled->soil_latch;
    soil_type = soil->soil_type;
    
    /* See if we can't move through this stuff. */
    if (soil->max_speeds_mps[WHEELED_MAX_FORWARD] == 0.0)
    {
	wheeled->state = WHEELED_STATE_STICKING;
	DEBUG_WHEELED(("Vehicle %d: About to get stuck in soil type %d\n",
		       vehicle_id, soil->soil_type));
    }
    else
    {
	/* See if we coasted through */
	if (wheeled->state == WHEELED_STATE_STICKING)
	  wheeled->state = WHEELED_STATE_HEALTHY;
    }

    /* Get slope */
    slope_sin = direction[Z];
    soil_slope = asin(fabs(slope_sin));
    soil_slope = RAD_TO_DEG(soil_slope);
    
    if (wheeled_mobility || wheeled_mobility_plus_allow_env)
    {
	cctt_soil =  whl_simnet_to_cctt_soil_mapping(wheeled, position,
						     soil->soil_type);
	if (cctt_soil < 0)
	  cctt_soil = 0;
	
	veh_max_speed = whl_get_veh_max_speed(wheeled, position, cctt_soil);
	
	if (desired_speed > veh_max_speed)
	  desired_speed = veh_max_speed; 
    }
    
    if ((wheeled_mobility || wheeled_mobility_plus_allow_env) &&
	(wheeled->state != WHEELED_STATE_START_TROUBLE) &&
	(wheeled->state != WHEELED_STATE_IN_TROUBLE))
    {
	if(whl_in_trouble_slope(soil_slope, wheeled, position, soil))
	{
	    wheeled->state = WHEELED_STATE_START_TROUBLE;
	    wheeled->waiting_in_trouble_state = time_last_simulation_clock;
	    wheeled->saved_desired_speed = desired_speed;
	}
    }

    if (wheeled->state == WHEELED_STATE_START_TROUBLE)
    {
	if ((wheeled->waiting_in_trouble_state + 7000) > 
	    time_last_simulation_clock)
	{
	    desired_speed = 0.0;
	}
	else
	{
	    wheeled->state = WHEELED_STATE_IN_TROUBLE;
	}
    }

    if(wheeled->state == WHEELED_STATE_IN_TROUBLE)
    {
	if (whl_in_trouble_slope(soil_slope, wheeled, position, soil))
	{
	    if ((wheeled->waiting_in_trouble_state + 
		 wheeled->params->in_trouble_limit) <
		(time_last_simulation_clock))
	    {
		desired_speed = wheeled->saved_desired_speed;
		wheeled->state = WHEELED_STATE_STICKING;
	    }
	}
	else
	  wheeled->state = WHEELED_STATE_HEALTHY;
    }

    /* 
     * ModSAF Vehicle Dynmaics Section
     *
     *   This section computes the ModSAF Wheeled Mobility Model dynamics.
     * 
     *        Input:
     *            wheeled->desired_dir  - desired direction
     *            desired_speed         - desired speed
     *
     *        Output:
     *            dx, dy                - direction
     *            position[]            - position
     */

    if (wheeled->params->mobility_model == WHL_MODSAF_MOBILITY_MODEL)
    {
	/* How fast can we go on this soil? */
	if (wheeled->current_gear == HULLS_GEAR_FORWARD)
	{
	    max_mps = soil->max_speeds_mps[WHEELED_MAX_FORWARD];
	    dz = direction[Z];
	}
	else
	{
	    max_mps = soil->max_speeds_mps[WHEELED_MAX_REVERSE];
	    dz = -direction[Z];
	}

	/* Adjust for grade */
	if(!wheeled->on_bridge)
	  if (dz > soil->max_climb_sin) /* Too steep */
	    max_mps = 0.0;		/* this isn't right */
	  else if (dz > 0.0)		/* Not downhill */
	    max_mps *= 1.0 - sqrt(dz / soil->max_climb_sin); /* Magic */

	/* Make our desired direction a 2D unit vector */
	flat_2D(wheeled->desired_dir[X], 
		wheeled->desired_dir[Y], 
		&wheeled->desired_dir[X], 
		&wheeled->desired_dir[Y]);

	desired_direction[X] = wheeled->desired_dir[X];
	desired_direction[Y] = wheeled->desired_dir[Y];

	/* get the pointer to the physical parameters structure for this vehicle */
	pdb = ent_get_physdb(vehicle_id);

	/* Calculate the level of dynamics resolution.  The algorithm for
	 * doing this is evolutionary.  Right now (1/17/95) it is very
	 * simple: a simple function of tick time located within a band.
	 * Outside the band the resolution level changes, within the band
	 * it doesn't. There is some dynamics to this, indicative of
	 * future algorithms.  They should all have some good dynamics,
	 * but not too complicated that they defeat the purpose of having
	 * multiple resolutions, which is to speed the program. 
	 */

	mini_driver(wheeled,
		    desired_speed, speed, dt, desired_direction, direction,
		    &current_gear,
		    &desired_wheel_angle, &desired_acceleration, &desired_brake, 
		    &desired_gear);
    
	/* Now that we have the desired inputs to the vehicle, let's
	 * calculate the dynamics of the vehicle.  Note that the only
	 * inputs to the dynamics function should be the only inputs that
	 * you could give to the actual vehicle. In this case these are
	 * desired wheel angle, desired acceleration, deisred braking, and
	 * desired gear.  However, we could put in desired seating
	 * position, lights, wipers, stereo volume, whatever.  The
	 * important thing is that they are inputs to an actual vehicle.
	 * The ONE exception (so far) is the desired complexity of the
	 * dynamics.  This should be put in as an argument in terms of
	 * programming only, NOT in terms of vehicle dynamics.  So, the
	 * complexity level will be passed, but only with great
	 * reluctance, since it makes things easier.  Of course, some
	 * vehicle parameters must be passed as well.  Also, the states of
	 * the vehicle must be passed (to be changed within the function).
	 * For now these states are speed, turn angle, and x and y
	 * position increments.
	 */
	dynamics(wheeled, pdb, max_mps, soil_type,
		 desired_speed,
		 desired_wheel_angle, desired_acceleration,
		 desired_brake, desired_gear, dt, position, &turn_angle,
		 &speed, &current_gear);

	/* compute direction components */
	dx = wheeled->direction[X] = cos(turn_angle);
	dy = wheeled->direction[Y] = sin(turn_angle);
    }
    /* 
     * 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:
     *            wheeled->dx,dy  - desired direction
     *            desired_speed   - desired speed
     *
     *        Output:
     *            dx, dy          - direction
     *            speed           - speed
     *
     */
    else if (wheeled->params->mobility_model == WHL_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 */

	float64      cctt_rot[2][2];               /* rotation matrix */
	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 */

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

        /* 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) wheeled_cctt_get_table_value(
	    &wheeled_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 - wheeled->last_check_time >
	   CHECK_PERIOD)
	{
	    wheeled->last_check_time = time_last_simulation_clock;
	    wheeled->within_hydro_surface =
	      ctdb_point_within_abstract(ctdb,
					 CTDB_ABSTRACT_HYDRO_SURFACE,
					 position[X], position[Y], &data) &&
	      data->hydro_surface.wetness;
	    wheeled->within_hydro_subsurface =
	      ctdb_point_within_abstract(ctdb,
					 CTDB_ABSTRACT_HYDRO_SUBSURFACE,
					 position[X], position[Y], &data) &&
	      data->hydro_subsurface.wetness;
	}

	if(wheeled->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 &&
	    !wheeled->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 ||
		wheeled->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) wheeled_cctt_get_table_value(&wheeled_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 = wheeled_cctt_get_table_value(
	    &wheeled_cctt_table_max_speed, cctt_index);

	/* Determine Local Gear */
	cctt_local_gear = wheeled->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(wheeled->desired_dir[X],  wheeled->desired_dir[Y], 
		&wheeled->desired_dir[X], &wheeled->desired_dir[Y]);
	flat_2D(direction[X], direction[Y], &dx, &dy);

	/* Determine Heading Error */
	cctt_heading_error = 
	  vmat2e_angle_between_vectors64(dx, dy, 
					 wheeled->desired_dir[X], 
					 wheeled->desired_dir[Y]);

	/* Determine Heading Error Rate */
	cctt_heading_error_rate = wheeled->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 = wheeled_cctt_get_table_value(
	    &wheeled_cctt_table_max_turn_rate, cctt_index);

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

        /* NONHOLONOMIC CONSTRAINT: Limit turn angle based on minimum turn radius */
 	if      (cctt_heading_error_rate >  wheeled->params->turn_radius_inv*speed)
 	  cctt_heading_error_rate =  wheeled->params->turn_radius_inv*speed;
 	else if (cctt_heading_error_rate < -wheeled->params->turn_radius_inv*speed)
 	  cctt_heading_error_rate = -wheeled->params->turn_radius_inv*speed;

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

	/* Compute New Angle */
	vmat2_primary_rotation64(sin(turn_angle), cos(turn_angle), cctt_rot);
	odx = dx, ody = dy;
	dx = odx * cctt_rot[X][X] + ody * cctt_rot[Y][X];
	dy = odx * cctt_rot[X][Y] + ody * cctt_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 (soil->max_speeds_mps[WHEELED_MAX_FORWARD] > 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;

	    wheeled->commanded_throttle_pos = 0.0;
	}

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

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

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

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

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

	/* limit throttle position */
	if      (wheeled->throttle_pos > 1.0)
	  wheeled->throttle_pos = 1.0;
	else if (wheeled->throttle_pos < 0.0)
	  wheeled->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(wheeled->throttle_pos, 0.0, 1.0,
				       wheeled->params->n_throttle_positions);
	cctt_index[3] = cctt_get_index(cctt_slope, 0.0, PI_4, 
				       wheeled->params->n_slopes);
	cctt_index[4] = cctt_get_index(speed, 0.0, cctt_max_speed, 
				       wheeled->params->n_speeds);
	cctt_force_drive = wheeled_cctt_get_table_value(&wheeled_cctt_table_force_drive, cctt_index);


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

        /* Determine Braking Force */
	if ((speed > WHEELED_CCTT_THRESHOLD) && 
	    (cctt_brake_pos_percent > WHEELED_CCTT_THRESHOLD_BRAKE))
	{
	    cctt_index[0] = cctt_vehicle_class;
	    cctt_index[1] = cctt_soil_type;
	    cctt_brake_linear_factor = wheeled_cctt_get_table_value(
                &wheeled_cctt_table_force_brake, cctt_index);
	    cctt_force_brake = WHEELED_CCTT_GRAVITY*pdb->mass_kg * cctt_brake_pos_percent * 
	      cctt_brake_linear_factor;
	}
	else
	  cctt_force_brake = 0.0;

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

	/* Ignore Gravity Going Downhill */
	if (((cctt_slope < 0.0) && 
	     (cctt_local_gear == WHEELED_CCTT_GEAR_FORWARD)) ||
	    ((cctt_slope > 0.0) && 
	     (cctt_local_gear == WHEELED_CCTT_GEAR_REVERSE)))
	{
	    cctt_weight_longitudinal = 0.0;
	    cctt_weight_vertical     = pdb->mass_kg*WHEELED_CCTT_GRAVITY;
	}
	  
	/* Determine Friction Force */
	cctt_index[0] = cctt_vehicle_class;
	cctt_index[1] = cctt_soil_type;
	cctt_mu_eff_coeff = wheeled_cctt_get_table_value(
	    &wheeled_cctt_table_friction, cctt_index);
	cctt_force_friction = cctt_mu_eff_coeff*cctt_weight_vertical;

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

	/* Sum Forces */
	if (cctt_local_gear == WHEELED_CCTT_GEAR_FORWARD) 
	{
	    cctt_force = cctt_force_drive - cctt_weight_longitudinal - 
	      cctt_force_friction - cctt_force_brake;

	}
	else if (cctt_local_gear == WHEELED_CCTT_GEAR_REVERSE)
	{
	    cctt_force = -cctt_force_drive - cctt_weight_longitudinal + 
	      cctt_force_friction + cctt_force_brake;

	}
	else
	  cctt_force = 0.0;

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

	/* Compute Speed */
	if (cctt_local_gear == WHEELED_CCTT_GEAR_FORWARD)
	{
	    speed += wheeled->acceleration*dt;
	}
	else
	{
	    speed -= wheeled->acceleration*dt;
	}
	speed = fabs(speed);

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

	if (wheeled->current_gear == HULLS_GEAR_REVERSE)
	{
	    speed = -speed;
	}

	/* Integrate position (this is included in the ModSAF dynamics subroutine)*/
	position[X] += speed * direction[X] * dt;
	position[Y] += speed * direction[Y] * dt;
    }

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


#if 0     
    /* Don't do anything expensive if we're already pointing the
     * right way.
     */
    if (right_direction)
    {
	dx = wheeled->desired_dir[X];
	dy = wheeled->desired_dir[Y];
	 
	/* Update our position for the distance we moved since
	 * last tick. 
	 */
	 
	if (!still)
	{
	    position[X] += velocity[X] * dt;
	    position[Y] += velocity[Y] * dt;
	}
    }
    else
    {
	/* Get our current direction in a 2D unit vector  */
	flat_2D(direction[X], direction[Y], &dx, &dy);
    }
#endif



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

	wheeled->bridge_collision_timeout = time_last_simulation_clock;

	ent_get_rotation_gcscs(wheeled->bridge_id, hull_to_world,
			       (int32)position[CELL3D]);

	/* 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) &&
            wheeled->node_look_fcn(wheeled,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, dx, dy,
			      &position[Z], hull_to_world, &new_soil,
			      &mes_key);

	if (saved != mes_key)
	{
	    saved_cell = ent_get_cell(vehicle_id);
	    saved = ent_get_mes_key(vehicle_id);
	    index = 0;
	    wheeled_set_current(wheeled, saved_cell, saved);
	}
        /* If this was not the correct enclosure start looking ahead in 
         * the list to see if the next enclosure (...) is the correct one
         */
        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 (!wheeled->node_look_fcn(wheeled,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, dx, dy,
                                  &position[Z], hull_to_world, &new_soil,
                                  &mes_key);
        }

	position[Z] += pdb->model_base_adjustment;
    }

    wheeled->g_component = hull_to_world[Y][Z];

    /* save our position so that we can see if it changed later..
     * (perhaps from a teleport of a vehicle. 
     */
    VMAT4_VEC_COPY64(position, wheeled->saved_position);
	
    new_soil = CTDB_SIMNET_SOIL(new_soil);

    /* If the soil changed, look up the new soil */
    if (new_soil != wheeled->params->soils[wheeled->soil_latch].soil_type)
      wheeled->soil_latch = wheeled_find_soil(new_soil, wheeled);

    /*
     * Set the entity parameters.. note that velocity[CELL3D] has already
     * been set by the ent_get_velocity_function...
     */
    if (wheeled->current_gear == HULLS_GEAR_FORWARD)
    {
	velocity[X] = hull_to_world[Y][X] * speed;
	velocity[Y] = hull_to_world[Y][Y] * speed;
	velocity[Z] = hull_to_world[Y][Z] * speed;
    }
    else
    {
	velocity[X] = -hull_to_world[Y][X] * speed;
	velocity[Y] = -hull_to_world[Y][Y] * speed;
	velocity[Z] = -hull_to_world[Y][Z] * speed;
	dust = 0;
    }

    ent_set_position_gcs(vehicle_id, position);
    DEBUG_WHEELED(("WHEELED position: (%.3lf, %.3lf, %.3lf) (mes_id: %d encl_id: %d)\n",
		   position[X], position[Y], position[Z], 
		   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]);

    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 * wheeled_fuel_usage_rate(wheeled->params, speed))
	== 0.0)
    {
	wheeled->state = WHEELED_STATE_NOGAS;
	cmpnt_available(vehicle_id, SM_WheeledHull, FALSE);
    }
    
    /* Reset the control inputs to zero at the end of each tick, so we
     * do not just wander off if we stop getting commands.
     */
#define NO_COMMAND_TIME 2000
    if ((time_last_simulation_clock - wheeled->last_command_time) >
	NO_COMMAND_TIME)
    {
	float64 dir[2];

	wheeled->control_state = WHEELED_EXPLICIT;
	vmat2e_unit64(hull_to_world[Y][X], hull_to_world[Y][Y], dir);

	wheeled->desired_dir[X] = dir[X];
	wheeled->desired_dir[Y] = dir[Y];
	wheeled->desired_dir[CELL3D] = cell;

	wheeled->speed = 0.0;
    }
    if (wheeledStats) 
      {
	float64 newDir[XYZC];
	ent_get_direction_gcs(vehicle_id, newDir);
	hulls_statsTickEnd(wheeledStats,         
			   vehicle_id,              
			   wheeled->desired_gear,              
			   wheeled->speed,                     
			   wheeled->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 = 0.0; 
	*yout = 0.0;
    }
    else
    {
	mag = 1.0 / fsqrt(mag);
	*xout = x * mag;
	*yout = y * mag;
    }
}



static void skew(
    int32  vehicle_id)    
{
    float64 skew[XYZ][XYZ];
    float64 htow[XYZ][XYZ];
    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(
    WHEELED_VARS *wheeled,   
    float64       position[XYZC])    
{
    uint32 soil;
    float64 adj_z = position[Z] + 1.0;  /* safety margin for multi-level */

    CTDB *ctdb = (CTDB*)gcs_get_tdb(position[CELL3D], FALSE);
    

    soil = CTDB_SIMNET_SOIL(ctdb_lookup_soil_ml(ctdb, position[X],
						position[Y], adj_z));

    wheeled->soil_latch = wheeled_find_soil(soil, wheeled);
}




int32 wheeled_find_soil(
    uint32        soil,   
    WHEELED_VARS *wheeled)    
{
    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<wheeled->params->n_soils;i++)
    {
	if (wheeled->params->soils[i].soil_type == soil)
	  return i;
	if (!wheeled->params->soils[i].soil_type)
	  zero = i;
    }
    return zero;
}



#if 0
static void tick_mini_driver(
    int32         vehicle_id,   
    WHEELED_VARS *wheeled,   
    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 wheeled->position is with respect to the
     * vehicles current cell!  Convert it in place...
     */

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

    /*
     * These are stubs.  Be sure to replace with better algorithms.
     */
    wheeled->max_accel = wheeled->max_turn = 0.0;
    switch (wheeled->control_state)
    {
      case WHEELED_POSITION_DIRECTION:
	dx = wheeled->position[X] - position[X];
	dy = wheeled->position[Y] - position[Y];

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

	    /* Face the point */
	    if (dx*direction[X] + dy*direction[Y] > 0)
	    {
		wheeled->desired_gear = HULLS_GEAR_FORWARD;
		wheeled->desired_dir[X] = dx;
		wheeled->desired_dir[Y] = dy;
	    }
	    else
	    {
		wheeled->desired_gear = HULLS_GEAR_REVERSE;
		wheeled->desired_dir[X] = -dx;
		wheeled->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.)
	     */
	    wheeled->speed = 0.5 * fsqrt(dx*dx + dy*dy) / dt;
	}
	break;

      case WHEELED_GOAL_CORRIDOR:
	/* This stub is worse that the previous one.  It just ignores the
	 * corridor altogether.
	 */
	wheeled->desired_dir[X] = wheeled->position[X] - position[X];
	wheeled->desired_dir[Y] = wheeled->position[Y] - position[Y];
	wheeled->desired_dir[CELL3D] = position[CELL3D];
	
	wheeled->desired_gear = HULLS_GEAR_FORWARD;
	wheeled->speed = wheeled->approach_speed;
	break;

      case WHEELED_TARGET_ID:
	/* Where is this guy? */
	ent_get_position_gcscs(wheeled->target_id, wheeled->position,
			       (int32)position[CELL3D]);

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

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


float64 wheeled_compute_eta(
    int32         vehicle_id,   
    WHEELED_VARS *wheeled,   
    float64      *target,   
    int32         cell)    
{
    struct wheeled_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 (wheeled->soil_latch == -1)
      soil = wheeled->params->soils; /* Use 0 by default */
    else
      soil = wheeled->params->soils+wheeled->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[WHEELED_MAX_FORWARD]);
}



/* ARGSUSED */
void wheeled_collision(
    int32                        vehicle_id,   
    COLLISION_NOTIFICATION_DATA *data,   
    ADDRESS                      user_data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);

    float64 position[XYZC];
    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 (!wheeled)		/* Passive error detection */
      return;

    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, wheeled->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 wheeled_bridge_collision(
    int32    vehicle_id,   
    int32    bridge_id,   
    ADDRESS  user_data)    
{
    float64 bridge_pos[XYZ], my_pos[XYZC];
    float64 dist1, dist2, dx, dy;

    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);

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

    /* Check to see if this bridge is closer than the current `bridge_id'.
     */
    if(wheeled->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(wheeled->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;
    }

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



static void wheeled_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(wheeled_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 wheeled_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;

    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);

    if (!wheeled)		/* 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;

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

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

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

    if (is_cat_kill)
    {
	if (damaged)
	{

	    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)wheeled->saved_position[CELL3D],
		    wheeled->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 ((wheeled->state == WHEELED_STATE_DEAD) ||
	    (wheeled->state == WHEELED_STATE_DYING))
	  return;
	else
	  wheeled->state = WHEELED_STATE_DYING;
    }
    else
    {
	if (wheeled->state == WHEELED_STATE_DEAD)
	{
	    wheeled->state = WHEELED_STATE_HEALTHY;
	    cmpnt_available(vehicle_id, SM_WheeledHull, TRUE);
	}
    }
}



#if 0    /* not used and not GCS aware */

/*  GET_RES_LEVEL
 *
 * $$$ Note: this function is never being called, and it has a
 * inherit problem with rel_level not being initialized to an
 * appropriate value.
 *
 * get_res_level computes the resolution level to be used in the
 * *current tick of dynamics.  It uses a neutral bacn of tick times.
 * 
 */

static int32 get_res_level(
    float64       delta_t,   
    WHEELED_VARS *wheeled)    
{
    /* declare variables */
    int32 tick_location, res_adder;
    
    int32 res_level = 0;     /* $$$ This does not make sense!! */

    /* check where tick time is: below, in, or above neutral band */
    if (delta_t < WHL_LOWER_TICK_BOUND) 
      tick_location = -1;	/* below */
    else if ((delta_t >= WHL_LOWER_TICK_BOUND) 
	     && (delta_t <= WHL_UPPER_TICK_BOUND))
      tick_location = 0;	/* within */
    else 
      tick_location = 1;	/* above */
  
    /* start a counter if the level is out of the band */
    if (tick_location == 1) 
      wheeled->res_tick_count++;
    else if (tick_location == 0) 
      wheeled->res_tick_count = 0;
    else if (tick_location == -1) 
      wheeled->res_tick_count--;
    else 
      wheeled->res_tick_count = wheeled->res_tick_count;

    /* if counter is at a certain value, increment or decrement, as 
       appropriate, the resolution, using a incrementer: res_adder */
    if (wheeled->res_tick_count >= 10) 
    {
	res_adder = -1;
	wheeled->res_tick_count = 0;
    }

    else if (wheeled->res_tick_count <= -10) 
    {
	res_adder = 1;
	wheeled->res_tick_count = 0;
    }
    else 
      res_adder = 0;
    /* increment (or decrement, etc.) the res level until it hits its 
       extremes */
    if ((res_level += res_adder) < WHL_MIN_RES_LEVEL) 
      res_level = WHL_MIN_RES_LEVEL;
    else if (res_level > WHL_MAX_RES_LEVEL) 
      res_level = WHL_MAX_RES_LEVEL;

    return(res_level);
}

#endif /* not used and not GCS aware */



/* MINI DRIVER
 *
 * mini_driver computes desired acceleration and desired wheel angle
 * based on the input desired speed and direction.  
 */
static void mini_driver(
    WHEELED_VARS *wheeled,   
    float64       desired_speed,   
    float64       speed,   
    float64       delta_t,   
    float64      *desired_direction,   
    float64      *direction,   
    int32        *current_gear,   
    float64      *desired_wheel_angle,   
    float64      *desired_acceleration,   
    float64      *desired_brake,   
    int32        *desired_gear)    
{

    /* change internal representation of gear position to my own (which makes
       more sense). */
    if (wheeled->desired_gear == HULLS_GEAR_REVERSE) 
      *desired_gear = -1;
    else if (wheeled->desired_gear == HULLS_GEAR_FORWARD)
      *desired_gear = 1;
    else if (wheeled->desired_gear == 0)
      *desired_gear = 0;
    else 
      *desired_gear = 2;

    if (wheeled->current_gear == HULLS_GEAR_REVERSE) 
      *current_gear = -1;
    else if (wheeled->current_gear == HULLS_GEAR_FORWARD) 
      *current_gear = 1;
    else if (wheeled->current_gear == 0) 
      *current_gear = 0;
    else 
      *current_gear = 2;

    /* compute desired wheel angle from angle between desired and
     * actual directions.  From this wheel angle, we may also wish to
     * determine if we desire reverse.  As when the desired wheel
     * angle is larger than 90 degrees.  In these cases it may be
     * simpler to move the vehicle backwards, turning the wheel in the
     * opposite direction.  Of course, then we have the problem of how
     * long a drive is acceptable in reverse.  We don't want to back
     * up 200 miles just because our desired direction is directly
     * behind us. Hmm. 
     */

    *desired_wheel_angle =
      vmat2e_angle_between_vectors64(direction[X], direction[Y],
				     wheeled->desired_dir[X], 
				     wheeled->desired_dir[Y]);

    if (fabs(*desired_wheel_angle) < .001)
      *desired_wheel_angle = 10.0 * *desired_wheel_angle;

    if (fabs(*desired_wheel_angle) >= 3.14)
    {
	*desired_gear = -1;
	desired_speed = 1.5;  /* This does not do anything to outside */
	*desired_wheel_angle = 0.0;
    } 
    else if (fabs(*desired_wheel_angle) > WHL_HALF_PI)
    {
	*desired_gear = -1;
	*desired_wheel_angle = (float64)SGN(*desired_wheel_angle) *
	  (PI - fabs(*desired_wheel_angle));
    }

    /*   {
     *desired_wheel_angle = 10.0 * *desired_wheel_angle;
     }  */

    /* check if desired gear must be changed from forward to reverse
     * or vice versa, in which case we'd like the driver to stop
     * before he changes the gears.  This is sort of cheating because
     * we want the driver to decide on his own that he needs to stop,
     * but for now it will do. 
     */
    if (SGN(*desired_gear) != SGN(*current_gear))
      desired_speed = 0.0;
 
    /* Then compute the desired acceleration based on a proportional
     * control algorithm modified by the tick time.  Note that this
     * acceleration is not direction dependent.  That is, negative
     * acceleration implies braking and positive acceleration implies
     * increasing engine speed (whether in a forward or reverse
     * gear). 
     */
    *desired_acceleration = (desired_speed-fabs(speed)) / delta_t;
 
    /* Here we calculate the desired brake pressure.  This one's going
     * to be tough to make realistic, since even the stupidest, most
     * non-intuitive drivers vary how they brake in an intuitive and
     * pretty complex way (according to weather and surface
     * conditions, etc.).  I am currently (as of 1/13/95) looking for
     * ideas on how to compute braking pressure.  For now, I will
     * leave it at a constant when the desired acceleration is less
     * than zero.  The braking pressure will be quantified in terms of
     * percent of maximum (maximum what? pedal travel). 
     */
    if (*desired_acceleration < 0.0) 
      *desired_brake = 50;
    else 
      *desired_brake = 0.0;
}




/*  DYNAMICS
 *   
 * dynamics computes the next tick of dynamics.  It outputs new
 * position, orientation, and speed, as well as wheel angle (for use
 * as well as wheel angle (for use in next tick).  It computes these
 * based on the resolution level computed in get_res_level.
 *
 * everything is assumed to be with respect to the same cell!
 */
static void dynamics(
    WHEELED_VARS *wheeled,   
    PHYSDB_DATA  *pdb,   
    float64       max_speed,   
    int32         soil_type,   
    float64       desired_speed,   
    float64       desired_wheel_angle,   
    float64       desired_acceleration,   
    float64       desired_brake,   
    int32         desired_gear,   
    float64       delta_t,   
    float64      *position,   
    float64      *turn_angle,   
    float64      *speed,   
    int32        *current_gear)    
{

    /* Declare local variables */
    float64 max_accel;
    float64 accel;
    float64 wheel_angle;
    float64 tan_inverse;
    float64 delta_radial, delta_tangential, delta_speed, delta_turn_angle;
    float64 IC_radius, radius_direction;
    float64 g_force, wheel_force, brake_force;
    float64 torque_to_wheel = 0.0;
    float64 wheel_radius = 0.0;
    float64 num_drive_wheels = 0.0;

    wheeled->resolution_level = 1;

    /* assign a nominal acceleration and deceleration to the vehicle
     * for use on the lowest resolution 
     */
    wheeled->nominal_acceleration = 3.0;
    wheeled->nominal_deceleration = -30.0;

    /* First thing to do is to check if the vehicle is in the desired
     * gear.  If the gears must change from a forward gear to reverse
     * or vice versa then the vehicle must stop.  From forward to
     * another forward, no stopping is necessary.  Later on I may add
     * in engine speed as a function of gear so that idiotic drivers
     * can blow up their engines.  Actually, what really must happen
     * is the gear box must stop spinning, right.  I mean you could be
     * slipping on ice and stop the wheels and put it into reverse
     * even if you're still moving forward.  We'll have to see.  Heck,
     * maybe I'll even let drivers grind their gears if that's not too
     * unreasonable, and they have manual transmission.  
     */

    if(desired_gear != *current_gear)
    {
	if ((SGN(desired_gear) != SGN(*current_gear)) && (fabs(*speed) > .001)) 
	  *current_gear = *current_gear;
	else 
	  *current_gear = desired_gear;
    }

    /* Compute actual acceleration based on desired acceleration and
     * gravitational load.  The gravitational load portion is
     * caclulated from the hull_to_world matrix.  I need to know the
     * weight of the vehicle for this calculation.  Also, I need to
     * figure out how I am going to incorporate it into the
     * acceleration calculation.  For now I am just going to use it to
     * modify the maximum acceleration, which is not too unreasonable,
     * since for now I am assuming that the "driver" is smart enough
     * to compensate (using the gas pedal) for any deviations in the
     * acceleration from that desired (see mini_driver above).  I
     * think I am going to make this a swappable block (2nd level).
     * Later, I'll incorporate it into a gas pedal model where the
     * driver must compensate for hills explicitly. (that'll be
     * another level) 
     */
    max_accel = wheeled->params->soils->max_accel_mps2;

    if (wheeled->resolution_level == 0)
    {
	if (desired_acceleration > 0.01) 
	  accel = wheeled->nominal_acceleration;
	else if (desired_acceleration < -0.01) 
	  accel = wheeled->nominal_deceleration;
	else 
	  accel = 0.0;
    }
    if (wheeled->resolution_level == 1)
    {
	if (max_accel != 0.0) 
	  max_accel = max_accel - WHL_G_CONST * wheeled->g_component;
	/* if we get a negative max_accel, vehicle drift uphill. this is bad */
	if (max_accel < 0)
	  max_accel = 0;
	if ((max_accel != 0.0) &&  (desired_acceleration > max_accel)) 
	  accel = max_accel;
	else if ((max_accel != 0.0) &&
		 (desired_acceleration <  -30.0*max_accel))
	  accel = -30.0*max_accel;
	else 
	  accel = desired_acceleration;
	                              
    }

    /* $$$ NOTE:  wheeled->resolution_level is always set to 1 at the
     * top of this routine, so this code will never be executed.  It
     * will not work properly if it is executed because wheel radius,
     * torque_to_wheel and num_drive_wheels are not initialized correctly.
     */
    if (wheeled->resolution_level == 2)
    {
	if (wheel_radius < .01) 
	{
	    fprintf(stderr,"Wheel radius is WAY too small");
	    exit(-1);
	}
	if (pdb->mass_kg < 50.0)
	{
	    fprintf(stderr,"Vehicle mass is WAY too small");
	    exit(-1);
	}

	/* First, calculate the net internally supplied force to the
	 * terrain from the wheels.  This includes engine power and
	 * brakes, since, in actuality, they can be applied at the
	 * same time (and are, in fact, on some traction control
	 * systems). 
	 */
	wheel_force = (torque_to_wheel / wheel_radius) * num_drive_wheels;
	brake_force = desired_brake * SGN(*current_gear) * SGN(*speed);
      
	/* Now, sum these to the gravity force and divide the sum by the
	 * mass of the vehicle to get the acceleration on "sticky" terrain. 
	 */
	g_force = WHL_G_CONST * (float64) pdb->mass_kg * wheeled->g_component;
	accel = (wheel_force + brake_force + g_force) / pdb->mass_kg;
      
	/* Now, we can "estimate" what size force the terrain can
	 * support.  That is, if it is largely hard terrain, we need
	 * the friction component on the wheels.  Once this is
	 * exceeded the wheels begin to slip and a new acceleration
	 * will be computed.  Once the wheels are slipping we then
	 * need to determine when they "catch" again as well as a new
	 * set of dynamics. 
	 */

    }

    /* check if vehicle length is too small, since we are dividing by it */
    if (pdb->dimensions[Y] < .0001) 
    {
	fprintf(stderr,"Vehicle length is WAY too small");
	exit(-1);
    }

    /* Compute actual steering angle based on steering linkage limits */
    if (fabs(desired_wheel_angle) > MAX_WHEEL_ANGLE)
      wheel_angle = MAX_WHEEL_ANGLE * (float64) SGN(desired_wheel_angle);
    else 
      wheel_angle = desired_wheel_angle;

    /*--------------------------------------------------*/

    /*--------------------------------------------------*/
    /* compute kinematics. */
    /* The following are lowest resolution kinematics */
    if (wheeled->resolution_level == 0)
    {
	/* position changes according to current direction and speed, 
	 * no steering angle input is used 
	 */
	position[X] += *speed * cos(*turn_angle) * delta_t;
	position[Y] += *speed * sin(*turn_angle) * delta_t;
      
	/* turn angle changes according to magnitude of wheel angle and 
	 * speed 
	 */
	delta_turn_angle = (*speed) * delta_t * wheel_angle * 
	  WHL_HALF_PI_INVERSE / (0.5 * pdb->dimensions[Y]);
    }
  
    /* The following are no-slip kinematics.  Automatic if either
     * resolution level=1 or if resolution level is >1 and the terrain
     * is of asphalt type (soil_type=1) (very "sticky"). 
     */
    if ((wheeled->resolution_level == 1) || 
	(soil_type == 1 && wheeled->resolution_level > 1)) 
    {
	/* check if wheel angle is zero (straight) so we don't divide by 
	   zero */
	if (fabs(wheel_angle) <= 0.000001)
	{
	    /* only position changes, not rotation */
	    position[X] += *speed * cos(*turn_angle) * delta_t *
	      SGN(*current_gear);
	    position[Y] += *speed * sin(*turn_angle) * delta_t *
	      SGN(*current_gear);
	    delta_turn_angle = 0.0;
	}

	/* check if the wheel angle is 90 degrees so we can deal with
	 * infinity.  In actuality, this should not happen, since the
	 * if-else block before this one disallows wheel angles
	 * greater than some specified value (which should be less
	 * than 90 degrees), but just in case, you know, if a vehicle
	 * is developed that can turn its wheels all the way around or
	 * something.
	 */
	else if (fabs(wheel_angle) >= 1.570795327)
	{
	    /* only rotation angle changes, not position */
	    delta_turn_angle = (*speed / pdb->dimensions[Y]) * delta_t *
	      (float64) SGN(wheel_angle);
	}
	else
	{
	    /* magnitude of instantaneous center of rotation */
	    tan_inverse = 1.0 / tan(wheel_angle);
	    /*printf("tan_inv: %.2f ",tan_inverse);*/
	    IC_radius = pdb->dimensions[Y] * 
	      sqrt(tan_inverse*tan_inverse + 0.25);
	    /* direction of radius from vehicle centroid to IC */
	    radius_direction = *turn_angle + (wheel_angle/2.0) + 1.570796327;

	    /* change in orientation of angle of vehicle */
	    delta_turn_angle = (*speed / IC_radius) * delta_t *
	      (float64) SGN(wheel_angle) * SGN(*current_gear);

	    /* change in position in vehicle coordinates */
	    delta_radial = IC_radius * (1.0 - cos(delta_turn_angle));
	    delta_tangential = (SGN(delta_turn_angle) * IC_radius * 
				sin(delta_turn_angle)) * SGN(*current_gear);

	    /* change in position in world coordinates */
	    position[X] += delta_radial * cos(radius_direction) +
	      delta_tangential * sin(radius_direction);
	    position[Y] += delta_radial * sin(radius_direction) + 
	      delta_tangential * cos(radius_direction) * -1.0;
	}
    }
    else if ((wheeled->resolution_level == 2) && (soil_type >2))
    {
        if (soil_type == 3)  /* soft sandy soil type */
        {
	    ;
	}  
    }

    /* compute new speed */
    if (wheeled->resolution_level != 0)
    {
	delta_speed = accel * delta_t;
	*speed += delta_speed;
    }
    else 
      *speed = desired_speed;

    if (fabs(*speed) > max_speed) 
      *speed = fabs(max_speed) * (float64) SGN(*speed);

    /* compute new turn angle */
    *turn_angle = *turn_angle + delta_turn_angle;
 
    /* Convert gear designations back to internal representation */
    if (*current_gear == -1)
    {
	wheeled->current_gear = HULLS_GEAR_REVERSE;
	wheeled->node_look_fcn  = 
	  (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_back;
    }
    else if (*current_gear == 0) 
      wheeled->current_gear = 0;
    else 
    {
	wheeled->current_gear = HULLS_GEAR_FORWARD;
	wheeled->node_look_fcn  = 
	  (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_ahead;
    }
    
}

static int32 whl_wet_weather_on(
    WHEELED_VARS        *wheeled,
    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 - wheeled->last_check_time >
       CHECK_PERIOD)
    {
	wheeled->last_check_time = time_last_simulation_clock;
      
	wheeled->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)) ||
	  wheeled->within_hydro_subsurface)
	return(TRUE);
      else
	return(FALSE);
  
    return(FALSE);
}

static int32 whl_in_trouble_slope(float64 soil_slope,
				  WHEELED_VARS *wheeled,
				  float64 position[XYZC],
				  struct wheeled_soils *soil)
{
    if(whl_wet_weather_on(wheeled, position) && 
       (soil_slope > soil->max_wet_slope))
      return TRUE;
  
    if(!whl_wet_weather_on(wheeled, position) && 
       (soil_slope > soil->max_dry_slope))
      return TRUE;
  
    return FALSE;
}

static int32 whl_simnet_to_cctt_soil_mapping(WHEELED_VARS  *wheeled,
					     float64       position[XYZC],
					     int32         soil_type)
{
    int32 i;

    /* This data is copied from libtracked. Will be updated when actual
     *  data received
     */
    if(whl_wet_weather_on(wheeled, position))
      /* wet weather */
    {
	for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++)
	{
	    if (wheeled_soil_tables[1].soil_table[i].simnet_soil_type
		== soil_type)
	      return(wheeled_soil_tables[1].soil_table[i].cctt_soil_type);
	}
	return(0);
    }
    else
    {
	for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++)
	{
	    if (wheeled_soil_tables[0].soil_table[i].simnet_soil_type
		== soil_type)
	      return(wheeled_soil_tables[0].soil_table[i].cctt_soil_type);
	}
	return(0);
    }
    return(0);  
}

static float64 whl_get_veh_max_speed(WHEELED_VARS  *wheeled,
				     float64       position[XYZC],
				     int32         cctt_soil)
{
    int32 i;

    /* This data is copied from libtracked. Will be updated when actual
     *  data received
     */
    if(whl_wet_weather_on(wheeled, position))
      /* wet weather */
    {
	for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++)
	{
	    if (wheeled_speed_tables[1].speed_table[i].simnet_soil_type
		== cctt_soil)
	      return(wheeled_speed_tables[1].speed_table[i].max_speed);
	}
	return(20.0);
    }
    else
    {
	for (i=0; i< WHEELED_MAX_SOIL_TYPE; i++)
	{
	    if (wheeled_speed_tables[0].speed_table[i].simnet_soil_type
		== cctt_soil)
	      return(wheeled_speed_tables[0].speed_table[i].max_speed);
	}
	return(29.0);
    }
}

/*
 * 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,            /* input value */
    float64            min,              /* minimum value */
    float64            max,              /* maximum value */
    int32              n_values)         /* 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;
}


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

void wheeled_statsOn(void)
{
  if (!wheeledStats) hulls_statsInit(&wheeledStats);
  else hulls_statsOn(wheeledStats);
  wheeled_statsStatus();
}

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

void wheeled_statsOff(void)
{
  if (wheeledStats) hulls_statsOff(wheeledStats);
  wheeled_statsStatus();
}

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

void wheeled_statsReset(void)
{
  if (wheeledStats) 
    {
      hulls_statsReset(wheeledStats);
      wheeled_statsStatus();
    }
  else printf("No wheeled stats memory allocated, use \"On\" command\n");
  
}

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

void wheeled_statsPrint(void)
{
  if (wheeledStats) 
    {
      printf("Wheeled stats:\n");
      hulls_statsPrint(wheeledStats);
    }
  else printf("No wheeled stats data, use \"on\" command\n");
}

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

void wheeled_statsStatus(void)
{
  printf("Wheeled stats ");
  hulls_statsStatus(wheeledStats);
  return;
}

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

void wheeled_statsQuit(void)
{
  if (wheeledStats) hulls_statsDestroy(&wheeledStats);
  wheeled_statsStatus();
}

