/******************************************************************/
/* 
Defines, typedefs, and structs
*/
/******************************************************************/
/*
New aspects of LATERAL model
1) hips are separated by a distance.
2) knees slide rather than rotate (calf shortens)
*/
/******************************************************************/

/* Indices */
#define LEFT 0
#define RIGHT 1

/* These are handy constants. XX rather than X to avoid math.h conflicts */
#define XX 0
#define YY 1
#define ZZ 2

/* Status flags */
#define STATUS_OK 0
#define CRASHED 1

/* May have N_CONTROLS != N_CTRL_INPUTS eventually
   include COM_X, COM_Y, TORSO_ROLL, or other variables in N_CTRL_INPUTS */
/* Joints (for trajectories and gains) */
#define L_HIP 0
#define L_KNEE 1
#define L_ANKLE 2
#define R_HIP 3
#define R_KNEE 4
#define R_ANKLE 5
#define N_BIPED_DOFS 6
#define N_CONTROLS 6
#define N_CTRL_INPUTS 6

/* This is used to allocate parameter vectors etc. */
#define MAX_N_PARAMETERS 100

/* This is used to allocate sdfast state */
#define MAX_N_SDFAST_STATE 100

// max state and command vector sizes
#define MAX_N_X 20
#define MAX_N_U 10

// foot status
#define FOOT_IN_AIR 0
#define FOOT_SLIDING 1
#define FOOT_ON_GROUND 2

/* dynamics_types (public version) */
#define INITIALIZE 3
#define DOUBLE_SUPPORT 2
#define SINGLE_SUPPORT 1
#define IN_AIR 0

// simulation type:
#define SIMULATION_TYPE_UNKNOWN 0
#define SDFAST_SPRINGS 1
#define SDFAST_CONSTRAINTS 2
#define SDFAST_IMPACTS 3

// some constants
#define GRAVITY 9.81

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

/* For each parameter read in from a file we have this information */
typedef struct parameter
{
  char *name;
  double value;
  int optimize;
  int regularize;
  double nominal_value;
  double regularize_weight;
  struct parameter *next;
  double *pointer;
} PARAMETER;

/******************************************************************/
// Dynamics structure

typedef struct {
  int n_x;
  int n_u;
  int valid; // flag: is this representation valid?
  double x[MAX_N_X];
  double u[MAX_N_U];
  double min_x[MAX_N_X];
  double max_x[MAX_N_X];
  double min_u[MAX_N_U];
  double max_u[MAX_N_U];
  double x_d[MAX_N_X];
  // may need to add simulation specific stuff here that doesn't
  // go into a lqrd state vector like quaternions or rotation matrices.
} Dynamics;

/******************************************************************/
/******************************************************************/
/******************************************************************/
/******************************************************************/
/******************************************************************/
/******************************************************************/
// Overall simulation structure.

typedef struct {

  // *** COMMON VARIABLES ACROSS ALL SIMULATIONS

  double time_step; // controller time step    
  double time;      // current time in simulation
  double duration;  // simulation duration
  int status;       // Are we CRASHED? 

  // Joint angles
  double roll; /* angle of torso wrt vertical */
  double hip_angle[2]; // 3D: hip_yaw[2], hip_roll[2], hip_roll[2]
  double knee_length[2]; // LATERAL-CHANGE
  double ankle_angle[2];

  // commanded torques
  double hip_command[2]; // 3D: hip_yaw_command[2], ...
  double knee_command[2];
  double ankle_command[2];

  // torques (actually applied)
  double hip_torque[2];
  double knee_force[2]; // LATERAL-CHANGE
  double ankle_torque[2];

  // Location of various body parts. */
  double head[3];
  double groin[3]; // LATERAL-CHANGE
  double hip[2][3]; // LATERAL-CHANGE
  double knee[2][3];
  double foot[2][3];
  // These are link COM locations
  double torso[3];
  double thigh[2][3];
  double calf[2][3];
  double com[3]; // total body COM

  double cop[3]; // overall center of pressure COP
  double cop_foot[2][3]; // center of pressure of each foot
  double com_to_cop[3]; // COM referenced to the COP.
  double cof[3]; // center of feet weighted by vertical force COF
  double com_to_cof[3]; // COM referenced to the COF.
  // I have forgotten what this is for. Goes into data file.
  double com_alt[3];

  // absolute angles
  double torso_abs_angle;
  double thigh_abs_angle[2];
  double calf_abs_angle[2];

  // joint angular velocities
  double rolld;
  double hip_angled[2];
  double knee_lengthd[2]; // LATERAL-CHANGE
  double ankle_angled[2];

  // Linear velocities
  double headd[3];
  double groind[3]; // LATERAL-CHANGE
  double hipd[2][3]; // LATERAL-CHANGE
  double kneed[2][3];
  double footd[2][3];
  // These are COM velocities
  double torsod[3];
  double thighd[2][3];
  double calfd[2][3];
  double comd[3];

  // *** KINEMATIC AND DYNAMIC PARAMETERS

  double torso_length;
  double torso_cm;
  double torso_mass;
  double torso_I;
  double pelvis_width;
  double thigh_length;
  double thigh_cm;
  double thigh_mass;
  double thigh_I;
  double calf_length;
  double calf_cm;
  double calf_mass;
  double calf_I;

  double total_mass;
  double total_weight_in_newtons;

  double head_offset[3]; // head relative to torso COM
  double hip_offset[2][3];  // hip relative to torso COM // LATERAL-CHANGE
  double knee_offset[3]; // knee relative to calf COM
  double foot_offset[3]; // foot relative to calf COM

  // *** DYNAMICS **************

  // Control bits
  /* Flag to indicate both feet off the ground not allowed */
  int in_air_not_allowed; // NEED TO IMPLEMENT THIS

  // Public interface
  int dynamics_type; // IN_AIR, SINGLE_SUPPORT, DOUBLE_SUPPORT
  // NEED TO FULLY IMPLEMENT THIS
  int foot_status[2]; // FOOT_IN_AIR, FOOT_ON_GROUND, FOOT_SLIDING, ...

  // ground forces
  double ground_force[2][3];

  // knee limit
  double knee_limit;
  double knee_limit_k;
  double knee_limit_b;

  // crash detection
  double hip_min;
  double hip_max;

  /* Ankle torque limits */
  double zmp_x_min;
  double zmp_x_max;

  /* force limits */
  double friction_cone_limit;

  // ground model
  double ground_level;

  // What type of simulation? springs, constraints, impact, etc. ?
  int simulation_type; 

  // SDFAST: except for sdfast_state, this stuff should be private
  /*
We are using states very close to sdfast states in the optimization.
Double support is different, since
sdfast does not not use minimal double support states.
This means the controller and the optimization stuff needs to
know about the sdfast stuff, and that only dynamics-impact.c
can be used. We need to be able to symmetrize sdfast states
in controller.c.
  */
  int sdfast_model; // Which model is multiple model system using?
  int ss_foot_down; // which foot is down
  double sdfast_state[MAX_N_SDFAST_STATE]; // sdfast state
  double sdfast_stated[MAX_N_SDFAST_STATE]; // derivative of sdfast state
  int sdfast_integration_steps_per_control_step;
  int sdfast_integration_steps_per_control_step_constraints;
  int sdfast_integration_steps_per_control_step_impact;
  double sdfast_ctol;
  double sdfast_baumgarte;
  double x_offset; // where is model (translation)

  // Ground model
  // ground spring damper parameters
  double ground_k[3];
  double ground_b[3];
  // original ground contact point.
  double foot_zero[2][3];

  // hack to stash transitions
  double stashed_ss_state[MAX_N_SDFAST_STATE];

  // used for optimization
  int disable_dynamics_transitions;

  // *** CONTROLLER **************

  // controller variables
  int controller_state;
  double cstate_start_time;
  double cstate_elapsed_time;

  // standard state (SS or DS, right flipped to left)
  int standard_state_type;
  double standard_state[MAX_N_SDFAST_STATE];

  // gains xxx[torque][sensor]
  double pos_gains[N_CONTROLS][N_CTRL_INPUTS];
  double vel_gains[N_CONTROLS][N_CTRL_INPUTS];
  double int_gains[N_CONTROLS][N_CTRL_INPUTS];

  // desired positions
  double head_d[3];
  double hip_d[3];
  double knee_d[2][3];
  double foot_d[2][3];
  double roll_d;
  double hip_angle_d[2];
  double knee_length_d[2]; // LATERAL-CHANGE
  double ankle_angle_d[2];

  // desired velocities
  double headd_d[3];
  double hipd_d[3];
  double kneed_d[2][3];
  double footd_d[2][3];
  double rolld_d;
  double hip_angled_d[2];
  double knee_lengthd_d[2]; // LATERAL-CHANGE
  double ankle_angled_d[2];

  // integrated errors
  double hip_integrated_error[2];
  double knee_integrated_error[2];
  double ankle_integrated_error[2];

  // Torque feedforwards
  double hip_command_ff[2];
  double knee_command_ff[2];
  double ankle_command_ff[2];

  /* Servo modes */
  int hip_servo_mode[2];
  int knee_servo_mode[2];
  int ankle_servo_mode[2];

  // roll controller parameters
  double wait_time;
  double launch_time;
  double torque1;
  double comd1;
  double hip_ff1;
  double knee_length_d1;
  double knee_extend1;
  double ds_torque_gain;
  double ss_torque_gain;
  double energy;
  double energy_d;
  double energy_threshold1;
  double predicted_x;
  double predicted_x_d;;
  double com_height_zero;
  double foot_width;
  double ds_torque;
  double stance_knee_pos_gain;
  double stance_knee_vel_gain;
  double stance_knee_int_gain;
  double swing_knee_pos_gain;
  double swing_knee_vel_gain;
  double swing_knee_int_gain;
  double touchdown_knee_pos_gain;
  double touchdown_knee_vel_gain;
  double touchdown_knee_int_gain;

  /* Perturbations */
  double torso_perturbation;

  /* debugging message flags */
  int controller_print;

  // *** OPTIMIZATION PARAMETERS ***************

  /* Objective function */
  double clearance_d; // desired clearance
  double ds_time_d; // desired double support time
  double ss_time_d; // desired single support time

  // optimization criterion weights
  double crashed_penalty_weight;
  double speed_penalty_weight;
  double torque_penalty_weight;
  double leg_force_scale_factor;
  double clearance_penalty_weight;
  double f_x_penalty_weight;
  double f_z_penalty_weight;
  double rolld_penalty_weight;
  double timing_weight;
  double impulse_weight;
  double asymmetry_weight;

  double summed_score; // sum of penalties over time
  double discount;
  double discount_to_power; // current discount factor for the trial
  double discounted_score; // sum of discounted penalties over time

  double torque_score; /* sum of torque penalties */
  double crashed_score; /* amount of time walker is crashed. */
  double knee_score; // penalty on bending knee wrong way.
  double speed_score; // penalty on not achieving desired speed
  double clearance_score; // penalty on having foot too low
  double f_x_score; // penalty on horizontal foot forces
  double f_z_score; // penalty on vertical foot forces
  double time_score; // penalty on transition time errors
  double impulse_score; // penalty on impulses during touchdown.
  double asymmetry_score; // penalty on an asymmetric gait.

  // scores for one step
  double one_step_torque_score;
  double one_step_knee_score;
  double one_step_speed_score;
  double one_step_clearance_score;
  double one_step_f_x_score;
  double one_step_f_z_score;
  double one_step_rolld_score;
  double one_step_crashed_score;
  double one_step_total_score;

  // scores for transitions
  double transition_time_score;
  double transition_impulse_score;
  double transition_asymmetry_score;

  // used for computing transition costs
  double ds_start_time;
  double ss_start_time;

  // used for computing transition costs
  int last_impulse_side;
  double last_impulse[3];
  double last_impulse_time;
  
  /* Useful variables for optimization */
  int n_parameters;
  double all_time_low_cost;
  double debug_criterion;
  int func_calls;
  int n_func_calls_per_eval;
  PARAMETER *params;
  char output_file[10000];
  int iter;

  // *** MISCELLANEOUS ***********************

  /*
    random stuff
    Make this thread safe: random number generator seed for rand_r() and rand()
  */
  int rand_seed;
  double rand_scale;

} SIM;

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

int other_side( int value );

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