/*****************************************************************************/
/*
  parameters.c: manage simulation parameters
*/
/*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <malloc.h>
#include <string.h>

#include "main.h"
#include "main2.h"

/*****************************************************************************/

void
init_default_parameters( SIM *s )
{
  int i;

  // infrastructure
  s->simulation_type = SIMULATION_TYPE_UNKNOWN;
  s->rand_scale = 0.0;
  s->rand_seed = 1;

  /* Overall parameters */
  s->duration = 20.0;
  s->time_step = 0.001;

  /* Objective function */
  s->clearance_d = 0.02;
  s->ds_time_d = 0.12;
  s->ss_time_d = 0.38;
  s->speed_penalty_weight = 0.0;
  s->torque_penalty_weight = 1e-6; // 1e-6 was too low: Zuu problem, 1e-3 okay
  s->leg_force_scale_factor = 0.1;
  s->clearance_penalty_weight = 10.0;
  s->f_x_penalty_weight = 1e-7;
  s->f_z_penalty_weight = 1e-7;
  s->crashed_penalty_weight = 100.0;
  s->timing_weight = 100.0;
  s->impulse_weight = 1.0;
  s->asymmetry_weight = 100.0;
  s->summed_score = 0.0;
  s->discount = 0.99954;
  s->discount_to_power = 1.0;
  s->discounted_score = 0;

  s->pos_gains[L_HIP][L_HIP] = 1000.0;
  s->vel_gains[L_HIP][L_HIP] = 500.0;
  s->int_gains[L_HIP][L_HIP] = 0.0;
  s->pos_gains[L_KNEE][L_KNEE] = 10000.0;
  s->vel_gains[L_KNEE][L_KNEE] = 10000.0;
  s->int_gains[L_KNEE][L_KNEE] = 1000.0;
  s->pos_gains[L_ANKLE][L_ANKLE] = 1000.0;
  s->vel_gains[L_ANKLE][L_ANKLE] = 500.0;
  s->int_gains[L_ANKLE][L_ANKLE] = 0.0;
  s->pos_gains[R_HIP][R_HIP] = 1000.0;
  s->vel_gains[R_HIP][R_HIP] = 500.0;
  s->int_gains[R_HIP][R_HIP] = 0.0;
  s->pos_gains[R_KNEE][R_KNEE] = 10000.0;
  s->vel_gains[R_KNEE][R_KNEE] = 10000.0;
  s->int_gains[R_KNEE][R_KNEE] = 1000.0;
  s->pos_gains[R_ANKLE][R_ANKLE] = 1000.0;
  s->vel_gains[R_ANKLE][R_ANKLE] = 500.0;
  s->int_gains[R_ANKLE][R_ANKLE] = 0.0;

  // contact impedance parameters
  s->ground_k[ZZ] = 300000.0;
  s->ground_b[ZZ] = 7000.0;
  s->ground_k[XX] = 1000000.0;
  s->ground_b[XX] = 200000.0;
  s->ground_level = 0.0;

  s->roll_d = 0.0;

  s->torso_perturbation = 0;

  s->torso_length = 0.8;
  s->thigh_length = 0.3918;
  s->calf_length = 0.3810;
  s->pelvis_width = 0.1778;
  s->torso_mass = 50;
  s->thigh_mass = 5.676;
  s->calf_mass = 6.8952;
  s->torso_I = 1.5;
  s->thigh_I = 0.09592;
  s->calf_I = 0.1535;
  s->torso_cm = 0.2868;
  s->thigh_cm = 0.1898;
  s->calf_cm = 0.2384;

  for( i = 0; i < 3; i++ )
    {
      s->head_offset[i] = 0;
      s->hip_offset[LEFT][i] = 0;
      s->hip_offset[RIGHT][i] = 0;
      s->knee_offset[i] = 0;
      s->foot_offset[i] = 0;
    }
  s->head_offset[ZZ] = s->torso_length - s->torso_cm;
  s->hip_offset[LEFT][XX] = -s->pelvis_width/2;
  s->hip_offset[LEFT][ZZ] = -s->torso_cm;
  s->hip_offset[RIGHT][XX] = s->pelvis_width/2;
  s->hip_offset[RIGHT][ZZ] = -s->torso_cm;
  s->knee_offset[ZZ] = s->calf_cm;
  // s->knee_offset[ZZ] = -(s->thigh_length - s->thigh_cm);
  s->foot_offset[ZZ] = -(s->calf_length - s->calf_cm);

  s->hip_min = 1.5*s->calf_length;
  s->hip_max = 1.1*(s->calf_length + s->thigh_length);

  /* Assume symmetric foot */
  s->zmp_x_min = -0.05;
  s->zmp_x_max = 0.05;
  // For learning code shut these limits off
  s->zmp_x_min = -1e10;
  s->zmp_x_max = 1e10;

  s->friction_cone_limit = 1e30;

  s->sdfast_integration_steps_per_control_step = 20;
  s->sdfast_integration_steps_per_control_step_constraints = 20;
  s->sdfast_integration_steps_per_control_step_impact = 1;
  s->sdfast_ctol = 1e-5; /* constraint tolerance */
  s->sdfast_baumgarte = 100.0; /* could be 10.0 - 1000.0 */

  s->n_func_calls_per_eval = 1;
  s->all_time_low_cost = 1e20;
  s->debug_criterion = 1;

  s->total_mass = (s->torso_mass + 2*s->thigh_mass + 2*s->calf_mass);
  s->total_weight_in_newtons = s->total_mass*9.81;

  s->controller_print = 0;

  s->last_impulse_side = -1;
  s->last_impulse_time = -1.0;

  s->wait_time = 0.1;
  s->wait_time = 0.0;
  s->launch_time = 0.3;
  s->launch_time = 0.23;
  s->launch_time = 0.29;

  s->predicted_x_d = 0.03;
  s->predicted_x_d = 0.06;
  s->com_height_zero = 0.8193;
  s->foot_width = 0.213;

  s->energy = 0.0;
  s->ds_torque = 0.0;

  // obsolete
  s->energy_threshold1 = -0.11;
  s->comd1 =    0.22;
  s->ds_torque_gain =    0.0;
  s->energy_d =    1.1;

  // learned and then fudged values
  s->predicted_x_d = 0.071194775403;
  s->torque1 = 5.345544338226;
  s->hip_ff1 = 94.322547912598;
  s->knee_length_d1 = 0.125589296222;
  s->knee_extend1 = 0.041431374848;
  s->ss_torque_gain = 1.002910971642;
  s->stance_knee_pos_gain = 10000.0;
  s->stance_knee_vel_gain = 10000.0;
  s->stance_knee_int_gain = 1000.0;
  s->swing_knee_pos_gain = 10000.0;
  s->swing_knee_vel_gain = 10000.0;
  s->swing_knee_int_gain = 1000.0;
  s->touchdown_knee_pos_gain = 10000.0;
  s->touchdown_knee_vel_gain = 10000.0;
  s->touchdown_knee_int_gain = 1000.0;
}

/*****************************************************************************/
/******************************************************************/

PARAMETER *malloc_parameter()
{
  PARAMETER *result;

  result = (PARAMETER *) malloc( sizeof( PARAMETER ) );
  if ( result == NULL )
    {
      fprintf( stderr, "No memory to allocate PARAMETER.\n" );
      exit( -1 );
    }
  result->name = NULL;
  result->value = 0;
  result->optimize = 0;
  result->regularize = 0;
  result->nominal_value = 0;
  result->regularize_weight = 0;
  result->next = NULL;
  result->pointer = NULL;
  return result;
}

/******************************************************************/

PARAMETER *read_parameter_file( char *filename )
{
  FILE *stream;
  char buffer[10000];
  PARAMETER *p, *top, *bottom;

  top = NULL;
  bottom = NULL;
  p = NULL;
  stream = fopen( filename, "r" );
  if ( stream == NULL )
    {
      fprintf( stderr, "Can't open %s\n", filename );
      exit( -1 );
    }
  for(;;)
    {
      /* get name of variable */
      if ( fscanf( stream, "%s", buffer ) < 1 )
	break;
      p = malloc_parameter();
      p->name = strdup( buffer );
      if ( fscanf( stream, "%lf", &(p->value) ) < 1 )
	{
	  fprintf( stderr, "Missing value for %s in %s\n",
		   p->name, filename );
	  exit( -1 );
	}

      /* read various commands */
      for( ; ; )
	{
	  if ( fscanf( stream, "%s", buffer ) < 1 )
	    {
	      fprintf( stderr, "Missing end for %s in %s\n",
		       p->name, filename );
	      exit( -1 );
	    }
	  if ( strcmp( buffer, "end" ) == 0 )
	    break;
	  if ( strcmp( buffer, "opt" ) == 0 )
	    p->optimize = 1;
	}

      if ( top == NULL )
	top = p;
      if ( bottom != NULL )
	bottom->next = p;
      bottom = p;
    }
  fclose( stream );
  return top;
}

/******************************************************************/
/******************************************************************/

int process_parameters( PARAMETER *p, SIM *s, int verbose )
{
  int total = 0;

  for( ; ; )
    {
      if ( p == NULL )
	break;
      /*
      else if ( strcmp( p->name, "" ) == 0 )
	p->pointer = &(s->);
      */
      else if ( strcmp( p->name, "launch_time" ) == 0 )
	p->pointer = &(s->launch_time);
      else if ( strcmp( p->name, "predicted_x_d" ) == 0 )
	p->pointer = &(s->predicted_x_d);
      else if ( strcmp( p->name, "torque1" ) == 0 )
	p->pointer = &(s->torque1);
      else if ( strcmp( p->name, "comd1" ) == 0 )
	p->pointer = &(s->comd1);
      else if ( strcmp( p->name, "hip_ff1" ) == 0 )
	p->pointer = &(s->hip_ff1);
      else if ( strcmp( p->name, "knee_length_d1" ) == 0 )
	p->pointer = &(s->knee_length_d1);
      else if ( strcmp( p->name, "knee_extend1" ) == 0 )
	p->pointer = &(s->knee_extend1);
      else if ( strcmp( p->name, "ds_torque_gain" ) == 0 )
	p->pointer = &(s->ds_torque_gain);
      else if ( strcmp( p->name, "ss_torque_gain" ) == 0 )
	p->pointer = &(s->ss_torque_gain);
      else if ( strcmp( p->name, "energy_d" ) == 0 )
	p->pointer = &(s->energy_d);
      else if ( strcmp( p->name, "stance_knee_pos_gain" ) == 0 )
	p->pointer = &(s->stance_knee_pos_gain);
      else if ( strcmp( p->name, "stance_knee_vel_gain" ) == 0 )
	p->pointer = &(s->stance_knee_vel_gain);
      else if ( strcmp( p->name, "stance_knee_int_gain" ) == 0 )
	p->pointer = &(s->stance_knee_int_gain);
      else if ( strcmp( p->name, "swing_knee_pos_gain" ) == 0 )
	p->pointer = &(s->swing_knee_pos_gain);
      else if ( strcmp( p->name, "swing_knee_vel_gain" ) == 0 )
	p->pointer = &(s->swing_knee_vel_gain);
      else if ( strcmp( p->name, "swing_knee_int_gain" ) == 0 )
	p->pointer = &(s->swing_knee_int_gain);
      else if ( strcmp( p->name, "touchdown_knee_pos_gain" ) == 0 )
	p->pointer = &(s->touchdown_knee_pos_gain);
      else if ( strcmp( p->name, "touchdown_knee_vel_gain" ) == 0 )
	p->pointer = &(s->touchdown_knee_vel_gain);
      else if ( strcmp( p->name, "touchdown_knee_int_gain" ) == 0 )
	p->pointer = &(s->touchdown_knee_int_gain);

      else 
	{
	  fprintf( stderr, "Don't recognize %s in process_parameters()\n", p->name );
	  exit( -1 );
	}
      *(p->pointer) = p->value;
      if ( p->optimize )
	{
	  if ( verbose )
	    printf( "%d: %s\n", total, p->name );
	  total++;
	}
      p = p->next;
    }

  if ( verbose )
    printf( "Optimizing %d parameters.\n", total );
  return total;
}

/************************************************************************/

void
parameters_to_vector( PARAMETER *p, float *v )
{
  int indx = 0;

  printf( "Parameters_to_vector:\n" );
  for( ; ; )
    {
      if ( p == NULL )
	break;
      if ( p->optimize )
	{
	  v[indx++] = *(p->pointer);
	  printf( "%d: %g\n", indx-1, v[indx-1] );
	}
      p = p->next;
    }
}

/************************************************************************/

void
parameters_to_dvector( PARAMETER *p, double *v )
{
  int indx = 0;

  printf( "Parameters_to_vector:\n" );
  for( ; ; )
    {
      if ( p == NULL )
	break;
      if ( p->optimize )
	{
	  v[indx++] = *(p->pointer);
	  printf( "%d: %lg\n", indx-1, v[indx-1] );
	}
      p = p->next;
    }
}

/************************************************************************/

void
vector_to_sim( float *v, int n_parameters, PARAMETER *p )
{
  int indx;

  for( indx = 0; indx < n_parameters; indx++ )
    {
      /* Find next parameter to optimize */
      for( ; ; )
	{
	  if ( p == NULL )
	    {
	      fprintf( stderr, "Failure in vector_to_parameters: %d\n",
		       indx );
	      exit( - 1 );
	    }
	  if ( p->optimize )
	    break;
	  p = p->next;
	}
      *(p->pointer) = v[indx];
      /*
      printf( "%d: %g\n", indx, v[indx] );
      */
      p = p->next;
    }
}

/************************************************************************/

void
dvector_to_sim( double *v, int n_parameters, PARAMETER *p )
{
  int indx;

  for( indx = 0; indx < n_parameters; indx++ )
    {
      /* Find next parameter to optimize */
      for( ; ; )
	{
	  if ( p == NULL )
	    {
	      fprintf( stderr, "Failure in vector_to_parameters: %d\n",
		       indx );
	      exit( - 1 );
	    }
	  if ( p->optimize )
	    break;
	  p = p->next;
	}
      *(p->pointer) = v[indx];
      /*
      printf( "%d: %g\n", indx, v[indx] );
      */
      p = p->next;
    }
}

/******************************************************************/

int write_param_file( char *filename, PARAMETER *p )
{
  FILE *stream;

  stream = fopen( filename, "w" );
  if ( stream == NULL )
    {
      fprintf( stderr, "Can't open %s for write.\n", filename );
      exit( -1 );
    }

  for( ; ; )
    {
      if ( p == NULL )
	break;
      fprintf( stream, "%s %18.12lf", p->name, *(p->pointer) );
      if ( p->optimize )
	fprintf( stream, " opt" );
      fprintf( stream, " end\n" );
      p = p->next;
    }

  fclose( stream );

  return 0;
}

/******************************************************************/

int write_param_file_c_code( char *filename, PARAMETER *p )
{
  FILE *stream;

  stream = fopen( filename, "w" );
  if ( stream == NULL )
    {
      fprintf( stderr, "Can't open %s for write.\n", filename );
      exit( -1 );
    }

  if ( p == NULL )
    {
      fclose( stream );
      return 0;
    }

  for( ; ; )
    {
      if ( p == NULL )
	break;
      fprintf( stream, "  s->%s = %25.20lf;\n", p->name, *(p->pointer) );
      p = p->next;
    }

  fclose( stream );

  return 0;
}

/*****************************************************************************/

