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

#include "libwhl_local.h"
#include <libreader.h>
#include <libparmgr.h>
#include <p_safmodels.h>
#include <stdext.h> /*common/include/global*/
#include <stdalloc.h> /*common/include/global*/
#include <stdlib.h>
#include <math.h>

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

static void    wheeled_cctt_read_table(
    char                        *path,        /* data path */
    char                        *filename,    /* filename of table */
    WHEELED_CCTT_TABLE          *table);      /* CCTT table structure RETURN */

WHEELED_PARAMETRIC_DATA wheeled_dummy_params = {
    2, {0.555, 277.8}, {0.125, 12.5} , 7000, 
       1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1, 1, 1, 1, 1,           
    1 , 
    { 0 ,
      { 20.0 , 20.0 } ,0.0, 0.0, 1.5 , 4.25 , 0.5 , 0.707 , 
      { 10.0 , 20.0 , 30.0 }}
};

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

    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, WHL_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(WHEELED_PARAMETRIC_DATA) +
	     (n_soils-1) * (sizeof(struct wheeled_soils)));

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

    /* Now finish filling in the fuel data */

    /* Fill in dummy values */
    bcopy(&wheeled_dummy_params, result, sizeof(wheeled_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;
	}

    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;
    
    /* 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 (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("turn_radius_inv"), 
				    my_entry, READER_TAGGED, 
				    READER_ERRORS_WITH_CONTEXT))
	  result->turn_radius_inv = 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 wheeled_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[WHEELED_MAX_FORWARD] =
	      KPH_TO_MPS(field[2].real);
	    result->soils[i].max_speeds_mps[WHEELED_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);
	}
    }

    return (ADDRESS) result;
}



static void changer(
    int32                    vehicle_id,
    WHEELED_PARAMETRIC_DATA *new_params)
{
    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 (new_params)
    {
	wheeled->soil_latch = -1;
	wheeled->params = new_params;
	wheeled->min_turn_radius =
	  WHEEL_BASE / tan(MAX_WHEEL_ANGLE); /* $$$ These should be parameters */
    }
}



void wheeled_init_params(void)
{
    pm_register_parm_fcns(SM_WheeledHull, parser, changer, NULL, NULL);
}



/*
 * wheeled_fuel_usage_rate ()
 *
 * This function does a linear interpolation on the fuel usage
 * data stored in the parameters.
 */
float64 wheeled_fuel_usage_rate(
    WHEELED_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;
}

/*
 * wheeled_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 wheeled_cctt_read_table(
    char *path,                 /* directory of table */
    char *filename,             /* filename of table */
    WHEELED_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,"wheeled 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,
	      "Wheeled: 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);
}

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

    /* read ModSAF/CCTT vehicle map table */
    wheeled_cctt_read_table(data_path,"cctt_whl_vehicle_map.tbl", 
			    &wheeled_cctt_table_vehicle_map);

    /* read soil map table */
    wheeled_cctt_read_table(data_path, "cctt_whl_soil_map.tbl",
			    &wheeled_cctt_table_soil_map); 
    
    /* read coefficient of friction table */
    wheeled_cctt_read_table(data_path, "cctt_whl_fric.tbl",
			    &wheeled_cctt_table_friction); 
    
    /* read drive force table */
    wheeled_cctt_read_table(data_path, "cctt_whl_drive.tbl",
			    &wheeled_cctt_table_force_drive);
    
    /* read brake deceleration table */
    wheeled_cctt_read_table(data_path, "cctt_whl_brake.tbl",
			    &wheeled_cctt_table_force_brake); 

    /* read max turn rate */
    wheeled_cctt_read_table(data_path, "cctt_whl_max_turn_rate.tbl",
			    &wheeled_cctt_table_max_turn_rate); 

    /* read max speed  */
    wheeled_cctt_read_table(data_path, "cctt_whl_max_speed.tbl",
			    &wheeled_cctt_table_max_speed); 
}

/*
 * wheeled_cctt_get_table_value()
 *
 * This function returns the value from the data table at 
 * index i_1, ..., i_k.
 */
float64 wheeled_cctt_get_table_value(
    WHEELED_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, "LIBWHEELED ERROR: attempted to read value beyond table range\n");
	exit(1);
    }

    return table->data[location];
}
