/****************************************************************************
*   File: trk_params.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: Code to manage parametric data for tracked
*       Created: Mon Jul 13 11:35:57 PDT 1992
*       Author: jesmith
*      $Revision$                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include <stdlib.h>
#include "libtrk_local.h"
#include <libreader.h>
#include <libconstants.h>
#include <libparmgr.h>
#include <libparmedit.h>
#include <libpduapi.h>
#include <p_safmodels.h>
#include <stdio.h>
#include <stdext.h> /*common/include/global*/
#include <stdalloc.h> /*common/include/global*/
#include <math.h>
#include <string.h>

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


#if 0
TRACKED_PARAMETRIC_DATA tracked_dummy_params = {
    8.0 , 1 ,
    { 0 ,
      { 20.0 , 20.0 } , 1.5 , 4.25 , 0.5 , 0.707 ,{ 10.0 , 20.0 , 30.0 } }
};
#endif
static void tracked_cctt_read_table(
    char *path,
    char *filename,
    TRACKED_CCTT_TABLE *table);


TRACKED_PARAMETRIC_DATA tracked_dummy_params = {
    2, {0.555, 277.8}, {0.125, 12.5} , 1 ,
    0.1, 6.0, 1.0, 1.0, 0.5, 6, 6, 30, 30, 6, 
    1, 7000,
    { 0 ,
      { 20.0 , 20.0 } , 40.0, 15.0, 1.5 , 4.25 , 0.5 , 0.707 , 
      { 10.0 , 20.0 , 30.0 }, 0 ,
      { 0 } , { 0.0 }
    }
};

struct tracked_editor_struct
{
    uint32      dynamics_type;
};


static ADDRESS parser(
    READER_UNION *libreader_format, 
    uint32 model_number, 
    int32 *size)
{
    READER_UNION *my_entry;
    TRACKED_PARAMETRIC_DATA *result;
    READER_UNION *soils, *field;
    int32 n_soils, i, len, hgt;

    my_entry = reader_find_tag((char *)model_number,
			       libreader_format,
			       READER_TAGGED,
			       READER_NO_ERRORS);

    if (!my_entry)
      return NULL;

    /*
     * Read in the fuel consumption table.
     */


    /* Get the fuel map data first, so we know how much space we'll need */
    if (field = reader_find_tag(reader_get_symbol("fuel_usage"), 
				my_entry, READER_TAGGED, 
				READER_ERRORS_WITH_CONTEXT))
    {
	len = NS_MIN(field[2].array[0].integer - 1, TRK_MAX_FUEL_DIM);
	hgt = NS_MIN(field[0].integer - 2, 2); /* speeds & rates */
    }
    else
    {
        len = 0;
	hgt = 0;
    }

    /* Do the rest of the fields */

    if (soils = reader_find_tag(reader_get_symbol("soils"), my_entry,
				READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
      n_soils = soils[0].integer - 2; /* length & tag */
    else
      n_soils = 0;

    if (!n_soils)
      n_soils = 1;

    *size = (sizeof(TRACKED_PARAMETRIC_DATA) +
	     (n_soils-1) * (sizeof(struct tracked_soils)));

    result =
      (TRACKED_PARAMETRIC_DATA *)STDALLOC(*size);

    /* Now finish filling in the fuel data */

    /* Fill in dummy values */
    bcopy(&tracked_dummy_params, result, sizeof(tracked_dummy_params));

    result->fuel_table_length = len;

    if (hgt == 2) /* if older .rdr file, fuel_usage is scalar */
	for (i=0;i<result->fuel_table_length;i++)
	{
	  result->fuel_speeds[i] = KPH_TO_MPS(field[2].array[i+1].real);
	  result->fuel_rates[i] = field[3].array[i+1].real / 3600.0;
	}

    /* CCTT Mobility Model parametric data */
    if (field = reader_find_tag(reader_get_symbol("mobility_model"), 
				my_entry, READER_TAGGED, 
				READER_NO_ERRORS))
      result->mobility_model = field[2].integer;
    else
      result->mobility_model = 1;

    if (field = reader_find_tag(reader_get_symbol("in_trouble_limit"), 
				my_entry, READER_TAGGED, 
				READER_ERRORS_WITH_CONTEXT))
      result->in_trouble_limit = field[2].integer;
    else
      result->in_trouble_limit = 180000;
    
    if (result->mobility_model == 2 || result->mobility_model == 3)
    {
	if (field = reader_find_tag(reader_get_symbol("throttle_forward_loop_gain"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->throttle_forward_loop_gain = field[2].real;
	if (field = reader_find_tag(reader_get_symbol("throttle_accel_feedback_gain"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->throttle_accel_feedback_gain = field[2].real;
	if (field = reader_find_tag(reader_get_symbol("tau_throttle_inv"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->tau_throttle_inv = field[2].real;
	if (field = reader_find_tag(reader_get_symbol("heading_error_rate_tau"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->heading_error_rate_tau = field[2].real;
	if (field = reader_find_tag(reader_get_symbol("reverse_drive_factor"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->reverse_drive_factor = field[2].real;
	if (field = reader_find_tag(reader_get_symbol("n_throttle_positions"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->n_throttle_positions = field[2].integer;
	if (field = reader_find_tag(reader_get_symbol("n_slopes"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->n_slopes = field[2].integer;
	if (field = reader_find_tag(reader_get_symbol("n_soil_types"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->n_soil_types = field[2].integer;
	if (field = reader_find_tag(reader_get_symbol("n_speeds"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->n_speeds = field[2].integer;
	if (field = reader_find_tag(reader_get_symbol("n_brake_positions"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->n_brake_positions = field[2].integer;

    }

    result->n_soils = n_soils;
    for (i=0;i<n_soils;i++)
    {
	/* Fill in dummy values */
	if (i>0)
	  bcopy(result->soils, result->soils+i,
		sizeof(struct tracked_soils));
	
	result->soils[i].soil_type = soils[i+2].array[1].integer;

	if (field =
	    reader_find_tag(reader_get_symbol("max_speeds"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	{
	    result->soils[i].max_speeds_mps[TRACKED_MAX_FORWARD] =
	      KPH_TO_MPS(field[2].real);
	    result->soils[i].max_speeds_mps[TRACKED_MAX_REVERSE] =
	      KPH_TO_MPS(field[3].real);
	}
	if (field =
	    reader_find_tag(reader_get_symbol("max_dry_slope"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	  result->soils[i].max_dry_slope = field[2].real;

	if (field =
	    reader_find_tag(reader_get_symbol("max_wet_slope"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	  result->soils[i].max_wet_slope = field[2].real;

	if (field =
	    reader_find_tag(reader_get_symbol("max_accel"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	  result->soils[i].max_accel_mps2 = field[2].real;
	if (field =
	    reader_find_tag(reader_get_symbol("max_decel"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	  result->soils[i].max_decel_mps2 = field[2].real;
	if (field =
	    reader_find_tag(reader_get_symbol("max_turn"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	  result->soils[i].max_turn_rps = DEG_TO_RAD(field[2].real);
	if (field =
	    reader_find_tag(reader_get_symbol("max_climb"), soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	  result->soils[i].max_climb_sin = sin(DEG_TO_RAD(field[2].real));
	if (field =
	    reader_find_tag(reader_get_symbol("dust_speeds"),
			    soils[i+2].array,
			    READER_TAGGED, READER_ERRORS_WITH_CONTEXT))
	{
	    result->soils[i].dust_speeds_mps[0] = KPH_TO_MPS(field[2].real);
	    result->soils[i].dust_speeds_mps[1] = KPH_TO_MPS(field[3].real);
	    result->soils[i].dust_speeds_mps[2] = KPH_TO_MPS(field[4].real);
	}

	/* now look for plow speeds & their data... */

	result->soils[i].n_plows = 0;

	if (field =
	    reader_find_tag(reader_get_symbol("plow_speeds"),
			    soils[i+2].array,
			    READER_TAGGED, READER_NO_ERRORS))
	{
	    uint32 j, k, type;
	    char whole_name[DEPLOYABLES_MAX_NAME_LENGTH+5];

	    /* loop through each array.. */
	    for (j=2; j<field[0].integer; j++)
	    {
		sprintf(whole_name, "part_%s", field[j].array[1].charptr);

		/* check to see if it is a valid plow name */
		if (type = const_name_to_value(reader_get_symbol(whole_name)))
		{
		    
		    /* yes.. ok. save the type and its data */
		    result->soils[i].plow_type[result->soils[i].n_plows] = 
		        type;
		    
		    for (k=0; k<2; k++)
		      result->soils[i].plow_data[result->soils[i].n_plows][k] =
			  KPH_TO_MPS(field[j].array[k+2].real);
		    
		    result->soils[i].n_plows++;
		    
		} else 
		  fprintf(stderr, "libtracked: unknown plow: %s\n",
			  field[j].array[1].charptr);
	    }
	} else
	  result->soils[i].n_plows = 0;

    }

    return (ADDRESS) result;
}



static void changer(
    int32                    vehicle_id,
    TRACKED_PARAMETRIC_DATA *new_params)
{
    TRACKED_VARS *tracked = (TRACKED_VARS *)
      class_get_user_data(vtab_get_vehicle(vehicle_id),
			  tracked_user_data_handle);

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

    if (new_params)
    {
	tracked->soil_latch = -1;
	tracked->params = new_params;
    }
}


static void p_to_e (
    TRACKED_PARAMETRIC_DATA      *p_data,
    struct tracked_editor_struct *e_data)
{
    e_data->dynamics_type = p_data->mobility_model;
}


static ADDRESS e_to_p (
    struct tracked_editor_struct *e_data,
    TRACKED_PARAMETRIC_DATA      *p_data,
    int32                        *size)
{
    TRACKED_PARAMETRIC_DATA  *result;

    *size = sizeof(TRACKED_PARAMETRIC_DATA) + 
               sizeof(struct tracked_soils) * (p_data->n_soils - 1);

    result = (TRACKED_PARAMETRIC_DATA*) STDALLOC(*size);
    bzero((char *)result, (unsigned int)*size);
    bcopy((char *)p_data, (char *)result, (int)*size);

    result->mobility_model = e_data->dynamics_type;

    return (ADDRESS)result;
}


void tracked_init_params(void)
{
    int32 token, format;

    pm_register_parm_fcns(SM_TrackedHull, parser, changer, NULL, NULL);

    /* Read in the PDUAPI bindings */
    pa_read_binding("trk_pardat.rdr", FALSE);

    /* Get the token for the structure */
    token = pa_get_token("TRACKED_PARAMETRIC_DATA");
    
    /* Define this format.  The only conversion performed is to */
    /* go from network order to host order and back.            */
    format = pa_define_conversion_format(token,token);

    pm_register_pa_formats(SM_TrackedHull, format, format);
    
    parmedit_register(SM_TrackedHull, 1, "trk_params.rdr",
		      sizeof(struct tracked_editor_struct),
		      p_to_e, e_to_p);
}



/*
 * tracked_fuel_usage_rate ()
 *
 * This function does a linear interpolation on the fuel usage
 * data stored in the parameters.
 */
float64 tracked_fuel_usage_rate(
    TRACKED_PARAMETRIC_DATA *params, 
    float64 speed)
{
    int32 sl, sh, len;
    float64 tmp, dp, *fr;

    if (!params->fuel_table_length)
      return 0.0;

    /* Save the length in register */
    len = params->fuel_table_length;
    fr = params->fuel_rates;

    /* Find the next lower and next higher percentages */
    for (sh=0;sh<len;sh++)
      if (params->fuel_speeds[sh] > speed)
	break;

    /* Check for boundary conditions */
    if (sh==len)
      sh = sl = len-1;
    else if (sh==0)
      sl = sh;
    else
      sl = sh - 1;

    if (params->fuel_speeds[sh] != params->fuel_speeds[sl])
        dp = ((speed - params->fuel_speeds[sl]) /
	      (params->fuel_speeds[sh] - params->fuel_speeds[sl]));
    else
        dp = 0.0;

    return (fr[sl] + dp * (fr[sh] - fr[sl]));

}


static FILE *path_fopen(
    char *data_path, 
    char *filename, 
    char *flags)
{
    char *path = strdup (data_path);
    char fullname[256];
    char *directory;
    FILE *fp = 0;

    for (directory = strtok (path, ":");
	 directory;
	 directory = strtok ((char *)0, ":"))
    {
	sprintf (fullname, "%s/%s", directory, filename);
	if ((fp = fopen (fullname, flags)) != 0)
	  break;
    }
    /* This is not a call to STDEDALLOC because there's no guarentee
       that strdup called the STDALLOC function. */
    free(path);
    return fp;
}


/*
 * tracked_cctt_read_table()
 *
 * This function reads a data table in the following form:
 *    (k)                   dimension of the table
 *    (n_0)                 size of first dimension
 *    (n_1)                 size of second dimension
 *
 *     ...                     ...
 *
 *    (n_k-1)               size of kth dimension
 *    (data_0,0,0, ..., 0)  data     
 *    (data_0,0,0, ..., 1)  data     
 *
 *     ...                     ...
 *
 *    (data_n0, n1, ..., nk-1) 
 * 
 */
static void tracked_cctt_read_table(
    char *path,                 /* directory of table */
    char *filename,             /* filename of table */
    TRACKED_CCTT_TABLE *table)  /* CCTT table structure RETURN */
{
    int32                i;
    FILE                *fp;
    char                 fullname[256], fc;

    /* open file */
    sprintf(fullname, "%s/%s", path, filename);
    if ((fp = path_fopen (path, filename, "r")) == NULL) 
    {
	fprintf(stderr,"tracked cctt error: unable to open file %s\n", 
		filename);
	exit(1);
    }
    
    /*
     * First line should be a comment, denoted by a '#' in column 1,
     * to contain the RCS revision string.  Since this isn't a reader
     * file, we have to handle this stuff ourselves.
     */
    fc = fgetc(fp);
    if (fc == '#')
    {
	/* Flush rest of line */
	while (!feof(fp))
	  if ((fc = fgetc(fp)) == '\n')
	    break;
    }
    else
      fprintf(stderr,
	      "Tracked: Expected comment absent from line 1 of %s.\n",
	      fullname);

    /* read dimension */
    fscanf(fp, "%d", &(table->dim));

    /* allocate storage for size of each dimension */
    table->n = (int32 *) STDALLOC(sizeof(int32)*(table->dim));

    /* read size of each dimension */
    for (i = 0; i < table->dim; i++)
    {
	fscanf(fp, "%d", &(table->n[i]));
    }

    /* allocate storage for data */
    table->size = 1;
    for (i = 0; i < table->dim; i++)
    {
	table->size *= table->n[i];
    }
    table->data = (float64 *) STDALLOC(sizeof(float64)*(table->size));

    /* read data */
    for (i = 0; i < table->size; i++)
    {
	fscanf(fp, "%lf", &(table->data[i]));
    }

    /* close file */
    fclose(fp);
}



/*
 * tracked_cctt_read_tables()
 *
 * This function reads the run-time CCTT data tables 
 */
void tracked_cctt_read_tables(char *data_path)
{

    /* read ModSAF/CCTT vehicle map table */
    tracked_cctt_read_table(data_path, "cctt_vehicle_map.tbl",
			    &tracked_cctt_table_vehicle_map);
    
    /* read soil map table */
    tracked_cctt_read_table(data_path, "cctt_soil_map.tbl",
			    &tracked_cctt_table_soil_map); 
    
    /* read coefficient of friction table */
    tracked_cctt_read_table(data_path, "cctt_fric.tbl",
			    &tracked_cctt_table_friction); 
    
    /* read drive force table */
    tracked_cctt_read_table(data_path, "cctt_drive.tbl",
			    &tracked_cctt_table_force_drive);
    
    /* read brake deceleration table */
    tracked_cctt_read_table(data_path, "cctt_brake.tbl",
			    &tracked_cctt_table_force_brake); 

    /* read max turn rate */
    tracked_cctt_read_table(data_path, "cctt_max_turn_rate.tbl",
			    &tracked_cctt_table_max_turn_rate); 

    /* read max speed  */
    tracked_cctt_read_table(data_path, "cctt_max_speed.tbl",
			    &tracked_cctt_table_max_speed); 
}



/*
 * tracked_cctt_get_table_value()
 *
 * This function returns the value from the data table at 
 * index i_1, ..., i_k.
 */
float64 tracked_cctt_get_table_value(
    TRACKED_CCTT_TABLE *table, /* CCTT table structure */ 
    int32 *index)              /* indices */
{
    int32                 i, size, location;

    size = 1;
    location = 0;
    for (i = table->dim-1; i >= 0; i--)
    {
	location += size*index[i];
	size *= table->n[i];
    }

    if (location >= table->size)
    {
	fprintf(stderr, "LIBTRACKED ERROR: attempted to read value "
		"beyond table range\n");
	exit(1);
    }

    return table->data[location];

}
