/****************************************************************************
*   File: whl_class.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: Manages class information attached to each vehicle        *
*       Created: Mon Jul 13 11:35:57 PDT 1992
*       Author: wtaylor
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "libwhl_local.h"
#include <libentity.h>
#include <libhulls.h>
#include <libsubcomp.h>
#include <libcomponents.h>
#include <libdfdam.h>
#include <libifdam.h>
#include <libchemdam.h>
#include <libcollision.h>
#include <libvecmat.h>
#include <libgcs.h>
#include <libtime.h>
#include <p_safmodels.h>
#include <mun_type.h>
#include <math.h>
#include <stdalloc.h> /*common/include/global*/
#include <stdext.h> /*common/include/global*/

int32 wheeled_user_data_handle;

static void wheeled_print(
    CLASS_USER_DATA_TYPE vars);

static void get_trouble_state(
    int32                           vehicle_id,
    int32                           user_data_handle,
    struct hulls_get_trouble_state *data);

static void wheeled_create(
    int32                    vehicle_id,   
    int32                    user_data_handle,   
    WHEELED_PARAMETRIC_DATA *params);    

static void wheeled_destroy(
    int32  vehicle_id,   
    int32  user_data_handle);    

static void set_dir_speed(
    int32                             vehicle_id,   
    int32                             user_data_handle,   
    struct hulls_set_direction_speed *data);    

static void set_vel_gear(
    int32                           vehicle_id,   
    int32                           user_data_handle,   
    struct hulls_set_velocity_gear *data);    

static void set_vel_dir(
    int32                                vehicle_id,   
    int32                                user_data_handle,   
    struct hulls_set_velocity_direction *data);    

static void set_external_control(
    int32                                vehicle_id,   
    int32                                user_data_handle,   
    struct hulls_set_velocity_direction *data);    

static void set_vel_ori(
    int32                                  vehicle_id,   
    int32                                  user_data_handle,   
    struct hulls_set_velocity_orientation *data);    

static void set_pos_dir(
    int32                                vehicle_id,   
    int32                                user_data_handle,   
    struct hulls_set_position_direction *data);    

static void set_goal_corr(
    int32                           vehicle_id,   
    int32                           user_data_handle,   
    struct hulls_set_goal_corridor *data);    

static void set_target_id(
    int32                       vehicle_id,   
    int32                       user_data_handle,   
    struct hulls_set_target_id *data);    

static void set_target_position(
    int32                             vehicle_id,   
    int32                             user_data_handle,   
    struct hulls_set_target_position *data);    

static void get_eta(
    int32                 vehicle_id,   
    int32                 user_data_handle,   
    struct hulls_get_eta *data);    

static void set_takeoff(
    int32                     vehicle_id,   
    int32                     user_data_handle,   
    struct hulls_set_takeoff *data);    

static void set_fly_level(
    int32                       vehicle_id,   
    int32                       user_data_handle,   
    struct hulls_set_fly_level *data);    

static void get_max_range(
    int32                       vehicle_id,   
    int32                       user_data_handle,   
    struct hulls_get_max_range *data);    

static void get_limits(
    int32                    vehicle_id,   
    int32                    user_data_handle,   
    struct hulls_get_limits *data);    

static void set_mes_key(
    int32                     vehicle_id,   
    int32                     user_data_handle,   
    struct hulls_set_mes_key *data);    

static void get_mes_key(
    int32                     vehicle_id,   
    int32                     user_data_handle,   
    struct hulls_get_mes_key *data);    

static void wheeled_started_being_moved(
    int32    vehicle_id,   
    int32    vehicle_moving_me,   
    int32    dynamics,   
    ADDRESS  user_data);    

static void wheeled_stopped_being_moved(
    int32    vehicle_id,   
    int32    vehicle_moving_me,   
    float64 *final_pos,   
    ADDRESS  user_data);    

void wheeled_init_subclass(void)
{
    wheeled_user_data_handle =
      class_reserve_user_data(disdbobj_class, "wheeled", wheeled_print);

    /* Tell libcomponents we are available. */
    cmpnt_define_instance(SM_WheeledHull, 1, &wheeled_user_data_handle,
			  (CMPNT_CREATE)wheeled_create,
			  (CMPNT_DESTROY)wheeled_destroy,
			  HULLS_SET_DIRECTION_SPEED_FCN, set_dir_speed,
			  HULLS_SET_VELOCITY_GEAR_FCN, set_vel_gear,
			  HULLS_SET_VELOCITY_DIRECTION_FCN, set_vel_dir,
			  HULLS_SET_VELOCITY_ORIENTATION_FCN, set_vel_ori,
			  HULLS_SET_POSITION_DIRECTION_FCN, set_pos_dir,
			  HULLS_SET_GOAL_CORRIDOR_FCN, set_goal_corr,
			  HULLS_SET_TARGET_ID_FCN, set_target_id,
			  HULLS_SET_TARGET_POSITION_FCN, set_target_position,
			  HULLS_GET_ETA_FCN, get_eta,
			  HULLS_SET_TAKEOFF_FCN, set_takeoff,
			  HULLS_SET_FLY_LEVEL_FCN,set_fly_level,
			  HULLS_GET_MAX_RANGE_FCN, get_max_range,
			  HULLS_SET_EXTERNAL_CONTROL_FCN, set_external_control,
			  HULLS_GET_LIMITS_FCN, get_limits,
			  HULLS_SET_MES_KEY_FCN, set_mes_key,
			  HULLS_GET_MES_KEY_FCN, get_mes_key,
			  HULLS_GET_TROUBLE_STATE_FCN, get_trouble_state,
			  A_END);

    /*
     * Register handlers for collisions.
     */
    callback_register_handler(coll_notify_event, wheeled_collision, NULL);
    callback_register_handler(coll_bridge_notify_event,
			      wheeled_bridge_collision, NULL);


    /*
     * Register handlers for receiving damage/repairs.
     */

    callback_register_handler(ent_kill_level_notify, wheeled_damage, 
			      NULL);
}



static char *state_string(
    WHEELED_STATE  x)    
{
    switch(x)
    {
      case WHEELED_STATE_HEALTHY:
	return "WHEELED_STATE_HEALTHY";
      case WHEELED_STATE_NOGAS:
	return "WHEELED_STATE_NOGAS";
      case WHEELED_STATE_STICKING:
	return "WHEELED_STATE_STICKING";
      case WHEELED_STATE_STUCK:
	return "WHEELED_STATE_STUCK";
      case WHEELED_STATE_DYING:
	return "WHEELED_STATE_DYING";
      case WHEELED_STATE_DEAD:
	return "WHEELED_STATE_DEAD";
    }
    return "?";
}


static char *control_state_string(
    WHEELED_CONTROL_STATE  x)    
{
    switch(x)
    {
      case WHEELED_EXPLICIT:
	return "WHEELED_EXPLICIT";
      case WHEELED_POSITION_DIRECTION:
	return "WHEELED_POSITION_DIRECTION";
      case WHEELED_GOAL_CORRIDOR:
	return "WHEELED_GOAL_CORRIDOR";
      case WHEELED_TARGET_ID:
	return "WHEELED_TARGET_ID";
      case WHEELED_TARGET_POSITION:
	return "WHEELED_TARGET_POSITION";
    }
    return "?";
}


#define NODE_PRINT(which,string) (i == wheeled->node_keys.which ? string :" ")
static void wheeled_print_node_key_list(
    WHEELED_VARS *wheeled)    
{
    int32 i;
    printf(" node_key history:\n");
    for (i=0;i<WHEELED_MAX_NODE_LIST_LENGTH;i++)
      printf("%s %s %s ==> mes %6d encl %6d cell %d\n",NODE_PRINT(current,"C"),
             NODE_PRINT(first,"F"), NODE_PRINT(last,"L"),
             CTDB_MES_NODE_KEY_TO_MES_ID(wheeled->node_keys.list[i]), 
             CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(wheeled->node_keys.list[i]),
	     wheeled->node_keys.list_cell[i]);

    if ((WHEELED_NODE_KEY_LOOK_FCN)wheeled->node_look_fcn == 
        (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_ahead )
      printf(" Node Key Function in look ahead mode\n");
    else
      printf(" Node Key Function in look back mode\n");

}


static void wheeled_print(
    CLASS_USER_DATA_TYPE  vars)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)vars;

    if (wheeled->soil_latch)
    {
	printf(" current limits (soil = %d):\n",
	       wheeled->params->soils[wheeled->soil_latch].soil_type);
	printf("    max_speeds_mps: forward %f, backward %f\n", 
	       wheeled->params->
	       soils[wheeled->soil_latch].max_speeds_mps[WHEELED_MAX_FORWARD],
	       wheeled->params->
	      soils[wheeled->soil_latch].max_speeds_mps[WHEELED_MAX_REVERSE]);
	printf("    max_accel_mps2: %f\n", 
	       wheeled->params->soils[wheeled->soil_latch].max_accel_mps2);
	printf("    max_decel_mps2: %f\n", 
	       wheeled->params->soils[wheeled->soil_latch].max_decel_mps2);
	printf("    max_turn_rps: %f\n", 
	       wheeled->params->soils[wheeled->soil_latch].max_turn_rps);
	printf("    sin(max_climb): %f\n", 
	       wheeled->params->soils[wheeled->soil_latch].max_climb_sin);
    }
    printf(" state: %s\n", state_string(wheeled->state));
    printf(" current gear: %s\n",
	   (wheeled->current_gear == HULLS_GEAR_FORWARD) ?
	   "forward" : "reverse");
    if (wheeled->state == WHEELED_STATE_STUCK)
      printf(" stuck position: <%f %f>\n",
	     wheeled->stuck_position[X], wheeled->stuck_position[Y]);
    printf(" control state: %s\n",
	   control_state_string(wheeled->control_state));
    switch(wheeled->control_state)
    {
      case WHEELED_GOAL_CORRIDOR:
	printf(" approach speed: %f\n", wheeled->approach_speed);
	printf(" corridor width: %f\n", wheeled->corridor_width);
	/* Fall through */
      case WHEELED_POSITION_DIRECTION:
	printf(" desired position: <%f %f>\n",
	       wheeled->position[X], wheeled->position[Y]);
	printf(" desired direction: <%f %f>\n",
	       wheeled->direction[X], wheeled->direction[Y]);
	break;
      case WHEELED_TARGET_ID:
	printf(" target id: %d\n", wheeled->target_id);
	/* Fall through */
      case WHEELED_TARGET_POSITION:
	printf(" target position: <%f %f>\n",
	       wheeled->position[X], wheeled->position[Y]);
    }
    printf(" desired direction: <%f %f>\n", 
	   wheeled->desired_dir[X], 
	   wheeled->desired_dir[Y]);
    printf(" desired speed: %f\n", wheeled->speed);
    printf(" desired gear: %s\n",
	   (wheeled->desired_gear == HULLS_GEAR_FORWARD) ?
	   "forward" : "reverse");
    printf(" max turn rate: %f\n", wheeled->max_turn);
    printf(" max acceleration: %f\n", wheeled->max_accel);
    wheeled_print_node_key_list(wheeled);
}

void wheeled_set_current(
    WHEELED_VARS      *wheeled,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  node_key)    
{
    CTDB_MES_NODE_KEY tmp;
    int32 i,look_ahead,look_back, cell;
    
    for (i=0,look_ahead=1,look_back=1;(look_ahead || look_back);i++)
    {
        if ( look_ahead )
        {
            look_ahead = wheeled_node_key_look_ahead(wheeled,i,&cell,&tmp);
            if (look_ahead && (tmp == node_key) && (cell == node_key_cell))
            {
                wheeled->node_keys.current = (wheeled->node_keys.current+i)%
                  WHEELED_MAX_NODE_LIST_LENGTH;
                return;
            }
        }
        
        if ( look_back )
        {
            look_back = wheeled_node_key_look_back(wheeled,i,&cell,&tmp);
            if ( look_back && (tmp == node_key) && (cell == node_key_cell))
            {
                wheeled->node_keys.current = ((wheeled->node_keys.current-i+
                                           WHEELED_MAX_NODE_LIST_LENGTH)%
                                          WHEELED_MAX_NODE_LIST_LENGTH);
                return;
            }
        }
    }

    /* Failed to find the key in the list */
    wheeled_clear_node_key_list(wheeled, node_key_cell, node_key);
}

static void wheeled_create(
    int32                    vehicle_id,   
    int32                    user_data_handle,   
    WHEELED_PARAMETRIC_DATA *params)    
{
    WHEELED_VARS *wheeled =
      (WHEELED_VARS *)STDALLOC(sizeof(WHEELED_VARS));

    bzero(wheeled, sizeof(WHEELED_VARS));

    class_set_user_data((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id),
			user_data_handle,
			(CLASS_USER_DATA_TYPE)wheeled);

    wheeled->soil_latch = -1;

    if (params)
      wheeled->params = params;
    else
      wheeled->params = &wheeled_dummy_params;

    wheeled->current_gear = HULLS_GEAR_FORWARD;
    wheeled->node_look_fcn  = 
      (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_ahead;

    
    /* this is a hack.  since we cannot find out the vehicles
     * current direction in di_create like we used to (we
     * shouldn't be calling another subclass's functions during
     * our subclasses create routine)..  so, we initialize `cells_initted'
     * to FALSE in our create routine indicating that it hasn't yet
     * been initialized.   during the first tick, it will be
     * initialized.
     */
    wheeled->cells_initted = FALSE;

    wheeled->desired_dir[CELL3D] = -1;
    wheeled->saved_position[CELL3D] = -1;

    wheeled->speed = 0.0;
    wheeled->desired_gear = HULLS_GEAR_FORWARD;
    wheeled->state = WHEELED_STATE_HEALTHY;
    wheeled->min_turn_radius =
      WHEEL_BASE / tan(MAX_WHEEL_ANGLE); /* $$$ These should be parameters */

    wheeled_set_current(wheeled, -1, ent_get_mes_key(vehicle_id));
    

    /*
     * Register callback to tick during the appropriate phase at the 
     * appropriate rate
     */
    callback_register_handler(
           disdbobj_get_tick_event(vehicle_id,
				   DISDBOBJ_PHYSICAL_HULL_MODELING,
				   0),
			      wheeled_tick,
			      (ADDRESS) NULL);
    /*
     * Register to check if we are being towed 
     */
    wheeled->ho_event_registration = ho_register_handlers(
	vehicle_id,
	NULL, NULL,
	NULL, NULL,
	wheeled_started_being_moved, NULL,
	wheeled_stopped_being_moved, NULL);
}


static void wheeled_destroy(
    int32  vehicle_id,   
    int32  user_data_handle)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    ho_unregister_handlers(&wheeled->ho_event_registration);

    class_set_user_data((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id),
			user_data_handle,
			(CLASS_USER_DATA_TYPE)0);

    STDDEALLOC(wheeled);
}    



static void set_dir_speed(
    int32                             vehicle_id,   
    int32                             user_data_handle,   
    struct hulls_set_direction_speed *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_EXPLICIT;

    wheeled->desired_dir[X] = data->direction[X];
    wheeled->desired_dir[Y] = data->direction[Y];
    wheeled->desired_dir[CELL3D] = data->cell;
    
    if (data->speed > 0.0)
    {
	wheeled->desired_gear = HULLS_GEAR_FORWARD;
	wheeled->speed = data->speed;
    }
    else
    {
	wheeled->desired_gear = HULLS_GEAR_REVERSE;
	wheeled->speed = -data->speed;
    }

    wheeled->max_turn = data->max_turn_rate;
    wheeled->max_accel = data->max_accel;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_external_control(
    int32                                vehicle_id,   
    int32                                user_data_handle,   
    struct hulls_set_velocity_direction *data)    
{
    /* This model doesn't do anything with this interface but instead
     * does exsctly what velocity/direction does.
     */
    set_vel_dir(vehicle_id, user_data_handle, data);
}


static void set_vel_gear(
    int32                           vehicle_id,   
    int32                           user_data_handle,   
    struct hulls_set_velocity_gear *data)    
{
    float64 mag;
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_EXPLICIT;

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

    mag = (data->velocity[X] * data->velocity[X] +
	   data->velocity[Y] * data->velocity[Y] +
	   data->velocity[Z] * data->velocity[Z]);

    /* Often the speed will not change between calls */
    if (mag != (wheeled->speed * wheeled->speed))
      wheeled->speed = fsqrt(mag);

    wheeled->desired_gear = data->gear;
    
    wheeled->max_turn = data->max_turn_rate;
    wheeled->max_accel = data->max_accel;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_vel_dir(
    int32                                vehicle_id,   
    int32                                user_data_handle,   
    struct hulls_set_velocity_direction *data)    
{
    float64 mag;
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_EXPLICIT;

    wheeled->desired_dir[X] = data->direction[X];
    wheeled->desired_dir[Y] = data->direction[Y];
    wheeled->desired_dir[CELL3D] = data->cell;
    
    mag = (data->velocity[X] * data->velocity[X] +
	   data->velocity[Y] * data->velocity[Y] +
	   data->velocity[Z] * data->velocity[Z]);

    /* Often the speed will not change between calls */
    if (mag != (wheeled->speed * wheeled->speed))
      wheeled->speed = fsqrt(mag);

    if ((data->velocity[X] * data->direction[X] +
	 data->velocity[Y] * data->direction[Y]) < 0.0)
      wheeled->desired_gear = HULLS_GEAR_REVERSE;
    else
      wheeled->desired_gear = HULLS_GEAR_FORWARD;

    if (data->max_turn_rates)
      wheeled->max_turn = data->max_turn_rates[0];
    else
      wheeled->max_turn = 0.0;

    wheeled->max_accel = data->max_accel;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_vel_ori(
    int32                                  vehicle_id,   
    int32                                  user_data_handle,   
    struct hulls_set_velocity_orientation *data)    
{
    float64 mag;
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_EXPLICIT;

    wheeled->desired_dir[X] = data->velocity[X];
    wheeled->desired_dir[Y] = data->velocity[Y];
    wheeled->desired_dir[CELL3D] = data->cell;
    
    mag = (data->velocity[X] * data->velocity[X] +
	   data->velocity[Y] * data->velocity[Y] +
	   data->velocity[Z] * data->velocity[Z]);

    /* Often the speed will not change between calls */
    if (mag != (wheeled->speed * wheeled->speed))
      wheeled->speed = fsqrt(mag);

    if (data->orientation &&
	((data->velocity[X] * cos(data->orientation[0]) +
	  data->velocity[Y] * sin(data->orientation[0])) < 0.0))
      wheeled->desired_gear = HULLS_GEAR_REVERSE;
    else
      wheeled->desired_gear = HULLS_GEAR_FORWARD;

    if (data->max_turn_rates)
      wheeled->max_turn = data->max_turn_rates[0];
    else
      wheeled->max_turn = 0.0;

    wheeled->max_accel = data->max_accel;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_pos_dir(
    int32                                vehicle_id,   
    int32                                user_data_handle,   
    struct hulls_set_position_direction *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_POSITION_DIRECTION;

    wheeled->position[X] = data->position[X];
    wheeled->position[Y] = data->position[Y];
    wheeled->position[CELL3D] = data->cell;
    
    wheeled->direction[X] = data->direction[X];
    wheeled->direction[Y] = data->direction[Y];
    wheeled->direction[CELL3D] = data->cell;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_goal_corr(
    int32                           vehicle_id,   
    int32                           user_data_handle,   
    struct hulls_set_goal_corridor *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_GOAL_CORRIDOR;

    wheeled->position[X] = data->position[X];
    wheeled->position[Y] = data->position[Y];
    wheeled->position[CELL3D] = data->cell;

    wheeled->direction[X] = data->direction[X];
    wheeled->direction[Y] = data->direction[Y];
    wheeled->direction[CELL3D] = data->cell;

    wheeled->approach_speed = data->approach_speed;
    wheeled->corridor_width = data->corridor_width;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_target_id(
    int32                       vehicle_id,   
    int32                       user_data_handle,   
    struct hulls_set_target_id *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_TARGET_ID;
    wheeled->target_id = data->id;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void set_target_position(
    int32                             vehicle_id,   
    int32                             user_data_handle,   
    struct hulls_set_target_position *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    wheeled->control_state = WHEELED_TARGET_POSITION;

    wheeled->position[X] = data->position[X];
    wheeled->position[Y] = data->position[Y];
    wheeled->position[CELL3D] = data->cell;

    wheeled->last_command_time = time_last_simulation_clock;
}


static void get_eta(
    int32                 vehicle_id,   
    int32                 user_data_handle,   
    struct hulls_get_eta *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    data->eta = wheeled_compute_eta(vehicle_id, wheeled, 
				    data->position, data->cell);
}


static void set_takeoff(
    int32                     vehicle_id,   
    int32                     user_data_handle,   
    struct hulls_set_takeoff *data)    
{
    float64 position[XYZC];
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); 

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

    ent_get_position_gcs(vehicle_id, position);
    position[Z] = data->altitude_agl;
    ent_set_position_gcs(vehicle_id, position);
}


static void set_fly_level(
    int32                       vehicle_id,   
    int32                       user_data_handle,   
    struct hulls_set_fly_level *data)    
{
    struct hulls_set_velocity_direction set_vel_dir_data;
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle);
    
    if (!wheeled) /* Passive error detection */
	return;

    /* Just call set_vel_dir routine - this routine does not really
     * make sense for a wheeled hull. 
     */

    set_vel_dir_data.cell = data->cell;

    set_vel_dir_data.velocity[X] = cos(data->speed);
    set_vel_dir_data.velocity[Y] = sin(data->speed);
    
    set_vel_dir_data.direction[X] = data->track[X];
    set_vel_dir_data.direction[Y] = data->track[Y];

    set_vel_dir_data.max_turn_rates = data->max_turn_rates;
    set_vel_dir(vehicle_id, user_data_handle, &set_vel_dir_data);

    wheeled->last_command_time = time_last_simulation_clock;
}



static void get_max_range(
    int32                       vehicle_id,   
    int32                       user_data_handle,   
    struct hulls_get_max_range *data)    
{
    float64                 fuel_available;
    float64                 speed;

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

    if (!wheeled) /* Passive error detection */
    {
	data->max_range = 0.0;
        return;
    }

    if ((fuel_available = scmp_get_amount(vehicle_id, munition_Fuel, 0)) 
         != 0.0)
    {
        speed = ent_get_speed(vehicle_id);
        data->max_range = fuel_available 
	    * wheeled_fuel_usage_rate (wheeled->params, speed); 
    }
    else
    {
        data->max_range = 0.0;
    }
}



static void get_limits(
    int32                    vehicle_id,   
    int32                    user_data_handle,   
    struct hulls_get_limits *data)    
{
    float64 pos[XYZC], dir[XYZ], htow[XYZ][XYZ], dz;
    int32 soil_type;
    struct wheeled_soils *soil;
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); 
    CTDB *ctdb = ent_get_ctdb(vehicle_id);
    int32  cell = ent_get_cell(vehicle_id);
    
    if (data->position)
    {
	/* check to make sure it is with respect to the correct cell.. */
	if (data->cell != cell)
	    gcs_gcs3_to_gcscs64(data->cell, data->position,
			      (int32)cell, pos);
	else
	    VMAT3_VEC_COPY64(data->position, pos);

	pos[CELL3D] = cell;
    }
    else
      ent_get_position_gcs(vehicle_id, pos);

    if (data->direction)
    {
	float64 tmp_dir[XYZ];
	
	/* have to make sure that data->cell is the same as the
	 * entities current cell.  if not, we have to convert.. 
	 */
	if (cell != data->cell)
	{
	    gcs_vec_to_vec64(data->cell, data->direction,
			     cell, tmp_dir);
	}
	else
	{
	    VMAT2_VEC_COPY64(data->direction, tmp_dir);
	}
	
        /* Big-wheel at that location */
        pos[Z] += 1.0;   /* safety margin for multi-level */
        ctdb_place_vehicle(ctdb, pos[X], pos[Y], 
			   1.0, 1.0,
			   tmp_dir[X], tmp_dir[Y],
			   pos+Z, htow, &soil_type);
        dz = htow[Y][Z];
    }
    else
    {
        ent_get_direction_gcs(vehicle_id, dir);
        dz = dir[Z];
        pos[Z] += 1.0;   /* safety margin for multi-level */
	soil_type = ctdb_lookup_soil_ml(ctdb, pos[X], pos[Y], pos[Z]);
    }

    soil_type = CTDB_SIMNET_SOIL(soil_type);

    if(wheeled->bridge_id)
      soil = wheeled->params->soils;
    else
      soil = wheeled->params->soils + wheeled_find_soil(soil_type, wheeled);

    /* Adjust for grade (lifted from whl_tick.c) */
    if(!wheeled->bridge_id)
    {
	if (dz > soil->max_climb_sin) /* Too steep */
	  data->max_speed = 0.0;
	else if (dz <= 0.0) /* Downhill */
	  data->max_speed = soil->max_speeds_mps[WHEELED_MAX_FORWARD];
	else
	  data->max_speed = (soil->max_speeds_mps[WHEELED_MAX_FORWARD] *
			     (1.0 - sqrt(dz / soil->max_climb_sin)));
    }
    else
      data->max_speed = soil->max_speeds_mps[WHEELED_MAX_FORWARD];

    data->max_accel  = soil->max_accel_mps2;
    data->max_decel  = soil->max_decel_mps2;
    data->max_turn   = soil->max_turn_rps;
    data->min_radius = wheeled->min_turn_radius;
    data->max_turn_accel = 0.0;	/* No Maximum */
    data->max_sideways = 0.0; /* can't move sideways */
    data->max_up     = 0.0; /* can't hover */
}


static void wheeled_started_being_moved(
    int32    vehicle_id,   
    int32    vehicle_moving_me,   
    int32    dynamics,   
    ADDRESS  user_data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);

    if (!wheeled)
      return;

    wheeled->being_moved = TRUE;
}

static void wheeled_stopped_being_moved(
    int32    vehicle_id,   
    int32    vehicle_moving_me,   
    float64 *final_pos,   
    ADDRESS  user_data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);
    if (!wheeled)
      return;

    wheeled->being_moved = FALSE;
    ent_get_direction_gcs(vehicle_id, wheeled->direction);
}


int32 wheeled_node_key_look_ahead(
    WHEELED_VARS      *wheeled,   
    uint32             how_far,   
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key)    
{
    int32 index, last;
    
    if (!how_far)
    {
	*node_key_cell =
	  wheeled->node_keys.list_cell[wheeled->node_keys.current];
        *node_key = wheeled->node_keys.list[wheeled->node_keys.current];
        return 1;
    }

    if (wheeled->node_keys.last <= wheeled->node_keys.current )
      last = wheeled->node_keys.last + WHEELED_MAX_NODE_LIST_LENGTH;
    else
      last = wheeled->node_keys.last;
    
    index = wheeled->node_keys.current + how_far;

    if ( index <= last )
    {
	*node_key_cell =
	  wheeled->node_keys.list_cell[index%WHEELED_MAX_NODE_LIST_LENGTH];
        *node_key =wheeled->node_keys.list[index%WHEELED_MAX_NODE_LIST_LENGTH];
        return 1;
    }
    
    *node_key_cell = GCS_ILLEGAL_CELL;
    *node_key = CTDB_INVALID_MES_NODE_KEY;
    return 0;
}

int32 wheeled_node_key_look_back(
    WHEELED_VARS      *wheeled,   
    uint32             how_far,   
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key)    
{
    int32 index, current;

    if (!how_far)
    {
	*node_key_cell =
	  wheeled->node_keys.list_cell[wheeled->node_keys.current];
        *node_key = wheeled->node_keys.list[wheeled->node_keys.current];
        return 1;
    }

    if (wheeled->node_keys.first >= wheeled->node_keys.current)
      current = wheeled->node_keys.current + WHEELED_MAX_NODE_LIST_LENGTH;
    else
      current = wheeled->node_keys.current;
    
    index = current - how_far;
    
    if ( index >= wheeled->node_keys.first)
    {
	*node_key_cell =
	  wheeled->node_keys.list_cell[index%WHEELED_MAX_NODE_LIST_LENGTH];
        *node_key =wheeled->node_keys.list[index%WHEELED_MAX_NODE_LIST_LENGTH];
        return 1;
    }
    
    *node_key_cell = GCS_ILLEGAL_CELL;
    *node_key = CTDB_INVALID_MES_NODE_KEY;
    return 0;
}
        
void wheeled_add_node_key_to_list(
    WHEELED_VARS      *wheeled,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  node_key)    
{
    /* Only add the key if it is not equal to the current node key */
    if (CTDB_MES_NODE_KEYS_EQUAL(wheeled->node_keys.list[wheeled->node_keys.
                                                         current],
                                 node_key) &&
	(wheeled->node_keys.list_cell[wheeled->node_keys.current] ==
	 node_key_cell))
      return;
    
    wheeled->node_keys.last = (wheeled->node_keys.current+1)%
      WHEELED_MAX_NODE_LIST_LENGTH;

    wheeled->node_keys.list_cell[wheeled->node_keys.last] = node_key_cell;
    wheeled->node_keys.list[wheeled->node_keys.last] = node_key;

    if (wheeled->node_keys.last == wheeled->node_keys.first)
      wheeled->node_keys.first = (wheeled->node_keys.first+1)%
        WHEELED_MAX_NODE_LIST_LENGTH;
}

void wheeled_clear_node_key_list(
    WHEELED_VARS      *wheeled,   
    int32              seed_cell,
    CTDB_MES_NODE_KEY  seed)    
{
    wheeled->node_keys.current = 0;
    wheeled->node_keys.last    = 0;
    wheeled->node_keys.first   = 0;
    wheeled->node_keys.list_cell[0] = seed_cell;
    wheeled->node_keys.list[0] = seed;
}


static void set_mes_key(
    int32                     vehicle_id,   
    int32                     user_data_handle,   
    struct hulls_set_mes_key *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  user_data_handle);

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

    DEBUG_WHEELED(("cur %d %d %d, next %d %d %d\n",
                   CTDB_MES_NODE_KEY_TO_MES_ID(data->mes_key), 
                   CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(data->mes_key),
		   data->mes_key_cell,
                   CTDB_MES_NODE_KEY_TO_MES_ID(data->next_mes_key),
                   CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(data->next_mes_key),
		   data->next_mes_key_cell));
           
    wheeled_set_current(wheeled, data->mes_key_cell, data->mes_key);
    wheeled_add_node_key_to_list(wheeled, data->next_mes_key_cell,
				 data->next_mes_key);
    DEBUG_WHEELED_EXEC(wheeled_print_node_key_list(wheeled));
}


static void get_mes_key(
    int32                     vehicle_id,   
    int32                     user_data_handle,   
    struct hulls_get_mes_key *data)    
{
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); 

    if (!wheeled) /* Passive error detection */
    {
	data->mes_key_cell = GCS_ILLEGAL_CELL;
        data->mes_key      = CTDB_INVALID_MES_NODE_KEY;
	data->next_mes_key_cell = GCS_ILLEGAL_CELL;
        data->next_mes_key = CTDB_INVALID_MES_NODE_KEY;
    }
    else
    {
        wheeled_node_key_look_ahead(wheeled, 0, &data->mes_key_cell,
				    &data->mes_key);
        wheeled_node_key_look_ahead(wheeled, 1, &data->next_mes_key_cell,
				    &data->next_mes_key);
    }
}


static void get_trouble_state(
    int32                           vehicle_id,
    int32                           user_data_handle,
    struct hulls_get_trouble_state *data)
{
    
    WHEELED_VARS *wheeled = (WHEELED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  wheeled_user_data_handle);
    

    if(!wheeled)
    {
	data->in_trouble_state = FALSE;
	return;
    }

    if(wheeled->state == WHEELED_STATE_IN_TROUBLE)
      data->in_trouble_state = TRUE;
    else
      data->in_trouble_state = FALSE;
}
