/****************************************************************************
*   File: libtrk_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 libtracked
*       Created: Mon Jul 13 11:35:57 PDT 1992
*       Author: jesmith
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

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

/* Short-hand debugging macro (assumes local variable vehicle_id) */
#define DEBUG_TRACKED(format) \
  CLASS_DEBUG((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), \
	      tracked_user_data_handle, format)

#define DEBUG_TRACKED_EXEC(routine) \
   CLASS_DEBUG_EXEC((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), \
                    tracked_user_data_handle, routine)

/* Mobility Model Constants */
#define TRK_MODSAF_MOBILITY_MODEL   1
#define TRK_CCTT_MOBILITY_MODEL     2
#define TRK_CCTT_MOBILITY_MODEL_X   3

#define TRACKED_CCTT_THRESHOLD               0.005
#define TRACKED_CCTT_GEAR_FORWARD            1
#define TRACKED_CCTT_GEAR_REVERSE            2
#define TRACKED_CCTT_GEAR_STOP               3
#define TRACKED_CCTT_GRAVITY                 9.81

#define TRACKED_MAX_NODE_LIST_LENGTH 10
typedef struct tracked_node_key_list
{
    int32 first;
    int32 last;
    int32 current;
    int32 list_cell[TRACKED_MAX_NODE_LIST_LENGTH];
    CTDB_MES_NODE_KEY list[TRACKED_MAX_NODE_LIST_LENGTH];
} TRACKED_NODE_KEY_LIST;

typedef int32 (*TRACKED_NODE_KEY_LOOK_FCN)(
    ADDRESS            tracked_vars,
    uint32             how_far,
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key);

typedef enum
{
    TRACKED_STATE_HEALTHY ,
    TRACKED_STATE_NOGAS ,
    TRACKED_STATE_STICKING ,
    TRACKED_STATE_STUCK ,
    TRACKED_STATE_DYING ,
    TRACKED_STATE_DEAD,
    TRACKED_STATE_START_TROUBLE,
    TRACKED_STATE_IN_TROUBLE
} TRACKED_STATE;

typedef enum
{
    TRACKED_EXPLICIT ,
    TRACKED_EXTERNAL ,
    TRACKED_POSITION_DIRECTION ,
    TRACKED_GOAL_CORRIDOR ,
    TRACKED_TARGET_ID ,
    TRACKED_TARGET_POSITION 
} TRACKED_CONTROL_STATE;

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

    /* Operating parameters */
    TRACKED_PARAMETRIC_DATA *params;
    
    /* State variable */
    TRACKED_STATE state;

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

    /* Last time command was given */
    uint32 last_command_time;

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

    /* $$$ DVW wave support */
    /* last time we yawed, in seconds */
    float64 time_til_yaw;

    float64 last_rotation[XYZ][XYZ];

    int32 roll_sign;

    /* Current gear */
    int32 current_gear;

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

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

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

    TRACKED_NODE_KEY_LOOK_FCN node_look_fcn;
    TRACKED_NODE_KEY_LIST     node_keys;

    /* CCTT Mobility Model X variables */
    float64 ccttx_heading_error_rate; /* heading error rate */
    float64 ccttx_sin_tangent_angle;  /* sin of tangent angle */

    uint32 last_check_time;
    int32  within_hydro_surface;
    int32  within_hydro_subsurface;

    int32 being_moved;

    HO_REGISTRATION_PTR ho_event_registration;

    int32 cells_initted;

    float64  sink_state[40];
} TRACKED_VARS;

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

typedef enum
{
    DRY_SOIL,
    WET_SOIL
} TRACKED_SOIL_CONDITION;

#define TRACKED_MAX_SOIL_CONDITION 2
#define TRACKED_MAX_SOIL_TYPE 15   /* total soil type found in include/global*/

typedef struct tracked_soil_table_type
{
    int32   simnet_soil_type;
    int32   cctt_soil_type;
} TRACKED_SOIL_TABLE_TYPE;

typedef struct tracked_soil_table
{
    TRACKED_SOIL_CONDITION soil_condition;
    TRACKED_SOIL_TABLE_TYPE soil_table[TRACKED_MAX_SOIL_TYPE];
} TRACKED_SOIL_TABLE;

/* VERY PRIVATE VARIABLES AND FUNCTIONS!  DO NOT USE OUTSIDE! */
extern int32 tracked_user_data_handle;
extern int32 tracked_mkill_cants_vehicle;
extern int32 tracked_wave_effect;
extern TRACKED_SOIL_TABLE tracked_soil_tables[TRACKED_MAX_SOIL_CONDITION];
extern void  tracked_init_params(void);
extern TRACKED_PARAMETRIC_DATA tracked_dummy_params;
extern float64 tracked_compute_eta(
    int32         vehicle_id,   
    TRACKED_VARS *tracked,   
    float64      *target,   
    int32         cell);    

extern float64 tracked_fuel_usage_rate(
    TRACKED_PARAMETRIC_DATA *params,   
    float64                  speed);    

extern int32   tracked_find_soil(uint32 soil, TRACKED_VARS *tracked);
extern void tracked_collision(
    int32                        vehicle_id,   
    COLLISION_NOTIFICATION_DATA *data,   
    ADDRESS                      user_data);    

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

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

extern void tracked_init_subclass(void);

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

extern READER_UNION *tracked_speeds;
extern float64 tracked_cctt_get_table_value(
    TRACKED_CCTT_TABLE *table,   
    int32              *index);    

extern void    tracked_cctt_read_tables(char *data_path);

extern TRACKED_CCTT_TABLE tracked_cctt_table_vehicle_map;
extern TRACKED_CCTT_TABLE tracked_cctt_table_soil_map;
extern TRACKED_CCTT_TABLE tracked_cctt_table_friction;
extern TRACKED_CCTT_TABLE tracked_cctt_table_force_drive;
extern TRACKED_CCTT_TABLE tracked_cctt_table_force_brake;
extern TRACKED_CCTT_TABLE tracked_cctt_table_max_turn_rate;
extern TRACKED_CCTT_TABLE tracked_cctt_table_max_speed;


extern char *tracked_plow_cat;    /* reader symbol name for plow */
extern char *tracked_bridge_cat;  /* reader symbol name for bridge */
extern char *tracked_plow_speeds_symbol;  
extern char *tracked_dirtmover_cat;
extern char *tracked_fwmr_symbol;
extern char *tracked_moldboard_symbol;
extern int32 tracked_mobility_plus_allow_env;
extern int32 tracked_mobility;



extern void tracked_add_node_key_to_list(
    TRACKED_VARS      *var,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  node_key);    

extern void tracked_clear_node_key_list(
    TRACKED_VARS      *var,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  initial_key);    

extern int32 tracked_node_key_look_back(
    TRACKED_VARS      *var,   
    uint32             how_far,   
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key);    

extern int32 tracked_node_key_look_ahead(
    TRACKED_VARS      *var,   
    uint32             how_far,   
    int32             *node_key_cell,
    CTDB_MES_NODE_KEY *node_key);    

extern void tracked_init_rao(char *data_path);

extern int32 tracked_have_rao(ObjectType guise);

extern void tracked_run_rao(
    int32         vehicle_id,   
    TRACKED_VARS *tracked,   
    ObjectType    guise,   
    float64       dt,   
    float32      *roll,   
    float32      *pitch,   
    float64       posit[4],
    float64       dir[3], 
    float64       vel[3]);

extern void tracked_set_current(
    TRACKED_VARS      *tracked,   
    int32              node_key_cell,
    CTDB_MES_NODE_KEY  node_key);    

extern int32 tracked_rao_veh_id_debug; /* id of vehicle to print debug info */

extern int32  tracked_sink_init(
    char  *data_path,
    uint32 flags);

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