/****************************************************************************
*   File: libwhl_local.h
*                                                                           *
*       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: Private header file for libwheeled
*       Created: Mon Jul 13 11:35:57 PDT 1992
*       Author: wtaylor
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "libwheeled.h"
#include <libvtab.h>
#include <libclass.h>
#include <libdisdbobj.h>
#include <libctdb.h>
#include <libreader.h>
#include <libcollision.h>
#include <libhandoff.h>
#include <stdtypes.h>

#define WHL_MODSAF_MOBILITY_MODEL            1
#define WHL_CCTT_MOBILITY_MODEL              2
#define WHEELED_CCTT_THRESHOLD               0.06
#define WHEELED_CCTT_THRESHOLD_BRAKE         0.01
#define WHEELED_CCTT_GEAR_FORWARD            1
#define WHEELED_CCTT_GEAR_REVERSE            2
#define WHEELED_CCTT_GEAR_STOP               3
#define WHEELED_CCTT_GRAVITY                 9.81

/* Short-hand debugging macro (assumes local variable vehicle_id) */
#define DEBUG_WHEELED(format) \
  CLASS_DEBUG((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), \
	      wheeled_user_data_handle, format)
#define DEBUG_WHEELED_EXEC(routine) \
   CLASS_DEBUG_EXEC((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), \
                    wheeled_user_data_handle, routine)


/* $$$ Constants which really should be parameters $$$ */
#define WHEEL_BASE          3.30       
#define MIN_WHEEL_ANGLE     0.05     
/* the most the vehicle can turn at max speed so that it doesn't tip over. */
#define MAX_WHEEL_ANGLE     0.4      
#define SMALL_WHEEL_ANGLE   0.0002

#define WHEELED_MAX_NODE_LIST_LENGTH 10
typedef struct wheeled_node_key_list
{
    int32 first;
    int32 last;
    int32 current;
    int32 list_cell[WHEELED_MAX_NODE_LIST_LENGTH];
    CTDB_MES_NODE_KEY list[WHEELED_MAX_NODE_LIST_LENGTH];
} WHEELED_NODE_KEY_LIST;

typedef int32 (*WHEELED_NODE_KEY_LOOK_FCN)(
    ADDRESS            wheeled_vars,
    uint32             how_far,
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key);

typedef enum
{
    WHEELED_STATE_HEALTHY ,
    WHEELED_STATE_NOGAS ,
    WHEELED_STATE_STICKING ,
    WHEELED_STATE_STUCK ,
    WHEELED_STATE_DYING ,
    WHEELED_STATE_DEAD,
    WHEELED_STATE_START_TROUBLE,
    WHEELED_STATE_IN_TROUBLE
} WHEELED_STATE;


typedef enum
{
    WHEELED_EXPLICIT ,
    WHEELED_POSITION_DIRECTION ,
    WHEELED_GOAL_CORRIDOR ,
    WHEELED_TARGET_ID ,
    WHEELED_TARGET_POSITION 
} WHEELED_CONTROL_STATE;



/* Per-vehicle wheeled class information */
typedef struct wheeled_vars
{
    /* Index into params structure of last soil type */
    int32 soil_latch;

    /* Operating parameters */
    WHEELED_PARAMETRIC_DATA *params;

    float64 nominal_acceleration;
    float64 nominal_deceleration;

    /* Computed from parameters */
    float64 min_turn_radius;

    /* State variable */
    WHEELED_STATE state;
    int32 resolution_level;

    float64 stuck_position[XYZC];
    float64 saved_position[XYZC];

    /* Last time tick was called */
    uint32 last_command_time;

   /* time entered in WHEELED_STATE_IN_TROUBLE state */
    uint32 waiting_in_trouble_state;
    float64 saved_desired_speed;

    /* Current gear */
    int32 current_gear;

    /* How the hull is being controlled */
    WHEELED_CONTROL_STATE control_state;

    /* counter for determining if tick time is out of bounds too long */
    int32 res_tick_count;

    /* Control parameters which we use to derive the parameters
     * which actually drive the dynamics.
     */
    float64 position[XYZC];
    float64 direction[XYZC];

    float64 approach_speed;
    float64 corridor_width;
    int32   target_id;

    /* gravity component on vehicle along wheelbase */
    float64 g_component;

    /* Desired parameters */
    float64 desired_dir[XYZC];
    float64 speed;
    int32   desired_gear;
    float64 max_turn;
    float64 max_accel;

    /* The id of the bridge we're on */
    int32  bridge_id;
    uint32 bridge_collision_timeout;
    int32  on_bridge;

    /* CCTT Mobility Model variables */
    float64 commanded_throttle_pos;   /* desired throttle position (from 0.0 to 1.0) */
    float64 throttle_pos;             /* actual throttle position (from 0.0 to 1.0) */
    float64 acceleration;             /* acceleration */

    WHEELED_NODE_KEY_LOOK_FCN node_look_fcn;
    WHEELED_NODE_KEY_LIST     node_keys;

    int32 being_moved;
  /*    CALLBACK_EVENT_PTR started_being_moved_event;
    CALLBACK_EVENT_PTR stopped_being_moved_event;
    */
    int32  within_hydro_surface;
    int32  within_hydro_subsurface; /* indicate is soil is wet */
    uint32 last_check_time;         /* holds last time soil wetness check*/
  
    HO_REGISTRATION_PTR ho_event_registration;

    int32 cells_initted;

} WHEELED_VARS;


#define WHEELED_MAX_SOIL_CONDITION 2
typedef enum
{
    DRY_SOIL,
    WET_SOIL
} WHEELED_SOIL_CONDITION;

typedef struct wheeled_soil_table_type
{
    int32 simnet_soil_type;
    int32 cctt_soil_type;
} WHEELED_SOIL_TABLE_TYPE;

#define WHEELED_MAX_SOIL_TYPE 15  /* total soil type found in include/global */

typedef struct wheeled_soil_table
{
    WHEELED_SOIL_CONDITION soil_condition;
    WHEELED_SOIL_TABLE_TYPE soil_table[WHEELED_MAX_SOIL_TYPE];
} WHEELED_SOIL_TABLE;

typedef struct wheeled_speed_table_type
{
    int32   simnet_soil_type;
    float64 max_speed;
} WHEELED_SPEED_TABLE_TYPE;

typedef struct wheeled_max_speed_table_type
{
    WHEELED_SOIL_CONDITION soil_condition;
    WHEELED_SPEED_TABLE_TYPE speed_table[WHEELED_MAX_SOIL_TYPE];
} WHEELED_SPEED_TABLE;

typedef struct wheeled_cctt_table {
    int32                dim;         /* dimension of table */
    int32                size;        /* total size of data table */
    int32               *n;           /* size of each dimension */
    float64             *data;        /* table data */
} WHEELED_CCTT_TABLE;

/* VERY PRIVATE VARIABLES AND FUNCTIONS!  DO NOT USE OUTSIDE! */
extern int32 wheeled_user_data_handle;
extern int32 wheeled_mkill_cants_vehicle;


extern WHEELED_SOIL_TABLE wheeled_soil_tables[WHEELED_MAX_SOIL_CONDITION];
extern WHEELED_SPEED_TABLE wheeled_speed_tables[WHEELED_MAX_SOIL_CONDITION];

extern void  wheeled_init_params(void);
extern WHEELED_PARAMETRIC_DATA wheeled_dummy_params;

extern float64 wheeled_compute_eta(
    int32         vehicle_id,   
    WHEELED_VARS *wheeled,   
    float64      *target,   
    int32         cell);    

extern float64 wheeled_fuel_usage_rate(
    WHEELED_PARAMETRIC_DATA *params,   
    float64                  speed);    

extern int32   wheeled_find_soil(
    uint32        soil,   
    WHEELED_VARS *wheeled);    

extern void wheeled_collision(
    int32                        vehicle_id,   
    COLLISION_NOTIFICATION_DATA *data,   
    ADDRESS                      user_data);    

extern void wheeled_bridge_collision(
    int32    vehicle_id,   
    int32    bridge_id,   
    ADDRESS  user_data);    

extern void wheeled_damage(
    int32    vehicle_id,   
    uint32   new_kill_level,   
    uint32   old_kill_level,   
    ADDRESS  user_data);    

extern void wheeled_init_subclass(void);

extern void wheeled_tick(
    int32    vehicle_id,   
    uint32   interval_since_last_tick,   
    ADDRESS  user_data);    

extern READER_UNION *wheeled_speeds;
extern int32 wheeled_mobility_plus_allow_env;
extern int32 wheeled_mobility;

extern void wheeled_add_node_key_to_list(
    WHEELED_VARS      *var,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  node_key);    

extern void wheeled_clear_node_key_list(
    WHEELED_VARS      *var,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  initial_key);    

extern int32 wheeled_node_key_look_back(
    WHEELED_VARS      *var,   
    uint32             how_far,   
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key);    

extern int32 wheeled_node_key_look_ahead(
    WHEELED_VARS      *var,   
    uint32             how_far,   
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key);    

extern void wheeled_set_current(
    WHEELED_VARS      *wheeled,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  node_key);    

extern float64 wheeled_cctt_get_table_value(
    WHEELED_CCTT_TABLE   *table,
    int32                *index);   

extern void    wheeled_cctt_read_tables(
    char                 *data_path);


extern WHEELED_CCTT_TABLE wheeled_cctt_table_vehicle_map;
extern WHEELED_CCTT_TABLE wheeled_cctt_table_soil_map;
extern WHEELED_CCTT_TABLE wheeled_cctt_table_friction;
extern WHEELED_CCTT_TABLE wheeled_cctt_table_force_drive;
extern WHEELED_CCTT_TABLE wheeled_cctt_table_force_brake;
extern WHEELED_CCTT_TABLE wheeled_cctt_table_max_turn_rate;
extern WHEELED_CCTT_TABLE wheeled_cctt_table_max_speed;
