/*****************************************************************************/
/*
  data.c: save data from simulation
*/
/*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "../useful/mrdplot/mrdplot.h"

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

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

extern SIM sim;

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

enum channels {
MRDPLOT_TIME=0,
MRDPLOT_L_HIP,
MRDPLOT_L_KNEE,
MRDPLOT_L_ANKLE,
MRDPLOT_R_HIP,
MRDPLOT_R_KNEE,
MRDPLOT_R_ANKLE,
MRDPLOT_L_HIP_D,
MRDPLOT_L_KNEE_D,
MRDPLOT_L_ANKLE_D,
MRDPLOT_R_HIP_D,
MRDPLOT_R_KNEE_D,
MRDPLOT_R_ANKLE_D,
MRDPLOT_L_HIPD,
MRDPLOT_L_KNEED,
MRDPLOT_L_ANKLED,
MRDPLOT_R_HIPD,
MRDPLOT_R_KNEED,
MRDPLOT_R_ANKLED,
MRDPLOT_L_HIPD_D,
MRDPLOT_L_KNEED_D,
MRDPLOT_L_ANKLED_D,
MRDPLOT_R_HIPD_D,
MRDPLOT_R_KNEED_D,
MRDPLOT_R_ANKLED_D,
MRDPLOT_COM_X,
MRDPLOT_COM_Z,
MRDPLOT_COM_XD,
MRDPLOT_COM_ZD,
MRDPLOT_COM_TO_COP_X,
MRDPLOT_COM_TO_COF_X,
MRDPLOT_COM_ALT_X,
MRDPLOT_X,
MRDPLOT_Z,
MRDPLOT_ROLL,
MRDPLOT_XD,
MRDPLOT_ZD,
MRDPLOT_ROLLD,
MRDPLOT_L_HIP_TORQUE,
MRDPLOT_L_KNEE_FORCE,
MRDPLOT_L_ANKLE_TORQUE,
MRDPLOT_R_HIP_TORQUE,
MRDPLOT_R_KNEE_FORCE,
MRDPLOT_R_ANKLE_TORQUE,
MRDPLOT_HEAD_X,
MRDPLOT_HEAD_Z,
MRDPLOT_GROIN_X,
MRDPLOT_GROIN_Z,
MRDPLOT_L_HIP_X,
MRDPLOT_L_HIP_Z,
MRDPLOT_R_HIP_X,
MRDPLOT_R_HIP_Z,
MRDPLOT_L_KNEE_X,
MRDPLOT_L_KNEE_Z,
MRDPLOT_R_KNEE_X,
MRDPLOT_R_KNEE_Z,
MRDPLOT_L_FOOT_X,
MRDPLOT_L_FOOT_Z,
MRDPLOT_R_FOOT_X,
MRDPLOT_R_FOOT_Z,
MRDPLOT_L_FOOT_FX,
MRDPLOT_L_FOOT_FZ,
MRDPLOT_R_FOOT_FX,
MRDPLOT_R_FOOT_FZ,
MRDPLOT_COP_X,
MRDPLOT_L_FOOT_COP_X,
MRDPLOT_R_FOOT_COP_X,
MRDPLOT_COF_X,
MRDPLOT_CSTATE,
MRDPLOT_CSTATE_ELAPSED_TIME,
MRDPLOT_LAST_IMPULSE_SIDE,
MRDPLOT_LAST_IMPULSE_X,
MRDPLOT_LAST_IMPULSE_Z,
MRDPLOT_STATUS,
MRDPLOT_SDFAST_MODEL,
MRDPLOT_SS_FOOT_DOWN,
MRDPLOT_L_FOOT_STATUS,
MRDPLOT_R_FOOT_STATUS,
MRDPLOT_TORSO_ABS_ANGLE,
MRDPLOT_L_THIGH_ABS_ANGLE,
MRDPLOT_L_CALF_ABS_ANGLE,
MRDPLOT_R_THIGH_ABS_ANGLE,
MRDPLOT_R_CALF_ABS_ANGLE,
MRDPLOT_L_HIP_COMMAND,
MRDPLOT_L_KNEE_COMMAND,
MRDPLOT_L_ANKLE_COMMAND,
MRDPLOT_R_HIP_COMMAND,
MRDPLOT_R_KNEE_COMMAND,
MRDPLOT_R_ANKLE_COMMAND,
MRDPLOT_ENERGY,
MRDPLOT_PREDICTED_X,
MRDPLOT_SDFAST_STATE0,
MRDPLOT_SDFAST_STATE1,
MRDPLOT_SDFAST_STATE2,
MRDPLOT_SDFAST_STATE3,
MRDPLOT_SDFAST_STATE4,
MRDPLOT_SDFAST_STATE5,
MRDPLOT_SDFAST_STATE6,
MRDPLOT_SDFAST_STATE7,
MRDPLOT_SDFAST_STATE8,
MRDPLOT_SDFAST_STATE9,
N_CHANNELS
};

#define MAX_N_POINTS 100000

int n_points = 0;
float data[MAX_N_POINTS*N_CHANNELS];

char *names[N_CHANNELS];
char *units[N_CHANNELS];

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

void init_data( SIM *s )
{
  int i;

  /*
  printf( "N_CHANNELS: %d; MRDPLOT_L_HIP: %d; last: %d\n", N_CHANNELS,
	  MRDPLOT_L_HIP, MRDPLOT_R_ANKLE_COMMAND );
  */

  for( i = 0; i < N_CHANNELS; i++ )
    {
      names[i] = "xxx";
      units[i] = "xxx";
    }

  names[MRDPLOT_TIME] = "time";

  names[MRDPLOT_L_HIP] = "l_hip";
  names[MRDPLOT_L_KNEE] = "l_knee";
  names[MRDPLOT_L_ANKLE] = "l_ankle";
  
  names[MRDPLOT_R_HIP] = "r_hip";
  names[MRDPLOT_R_KNEE] = "r_knee";
  names[MRDPLOT_R_ANKLE] = "r_ankle";

  names[MRDPLOT_L_HIP_D] = "l_hip_d";
  names[MRDPLOT_L_KNEE_D] = "l_knee_d";
  names[MRDPLOT_L_ANKLE_D] = "l_ankle_d";
  
  names[MRDPLOT_R_HIP_D] = "r_hip_d";
  names[MRDPLOT_R_KNEE_D] = "r_knee_d";
  names[MRDPLOT_R_ANKLE_D] = "r_ankle_d";
  
  names[MRDPLOT_L_HIPD] = "l_hipd";
  names[MRDPLOT_L_KNEED] = "l_kneed";
  names[MRDPLOT_L_ANKLED] = "l_ankled";
  
  names[MRDPLOT_R_HIPD] = "r_hipd";
  names[MRDPLOT_R_KNEED] = "r_kneed";
  names[MRDPLOT_R_ANKLED] = "r_ankled";
  
  names[MRDPLOT_L_HIPD_D] = "l_hipd_d";
  names[MRDPLOT_L_KNEED_D] = "l_kneed_d";
  names[MRDPLOT_L_ANKLED_D] = "l_ankled_d";
  
  names[MRDPLOT_R_HIPD_D] = "r_hipd_d";
  names[MRDPLOT_R_KNEED_D] = "r_kneed_d";
  names[MRDPLOT_R_ANKLED_D] = "r_ankled_d";

  names[MRDPLOT_COM_X] = "com_x";
  names[MRDPLOT_COM_Z] = "com_z";
  names[MRDPLOT_COM_XD] = "com_xd";
  names[MRDPLOT_COM_ZD] = "com_zd";
  names[MRDPLOT_COM_TO_COP_X] = "com_to_cop_x";
  names[MRDPLOT_COM_TO_COF_X] = "com_to_cof_x";
  names[MRDPLOT_COM_ALT_X] = "com_alt_x";

  names[MRDPLOT_X] = "x";
  names[MRDPLOT_Z] = "z";
  names[MRDPLOT_ROLL] = "roll";
  
  names[MRDPLOT_XD] = "xd";
  names[MRDPLOT_ZD] = "zd";
  names[MRDPLOT_ROLLD] = "rolld";
  
  names[MRDPLOT_L_HIP_TORQUE] = "l_hip_torque";
  names[MRDPLOT_L_KNEE_FORCE] = "l_knee_force";
  names[MRDPLOT_L_ANKLE_TORQUE] = "l_ankle_torque";
  
  names[MRDPLOT_R_HIP_TORQUE] = "r_hip_torque";
  names[MRDPLOT_R_KNEE_FORCE] = "r_knee_force";
  names[MRDPLOT_R_ANKLE_TORQUE] = "r_ankle_torque";
  
  names[MRDPLOT_HEAD_X] = "head_x";
  names[MRDPLOT_HEAD_Z] = "head_z";
  names[MRDPLOT_GROIN_X] = "groin_x";
  names[MRDPLOT_GROIN_Z] = "groin_z";
  names[MRDPLOT_L_HIP_X] = "l_hip_x";
  names[MRDPLOT_L_HIP_Z] = "l_hip_z";
  names[MRDPLOT_R_HIP_X] = "r_hip_x";
  names[MRDPLOT_R_HIP_Z] = "r_hip_z";
  names[MRDPLOT_L_KNEE_X] = "l_knee_x";
  names[MRDPLOT_L_KNEE_Z] = "l_knee_z";

  names[MRDPLOT_R_KNEE_X] = "r_knee_x";
  names[MRDPLOT_R_KNEE_Z] = "r_knee_z";

  names[MRDPLOT_L_FOOT_X] = "l_foot_x";
  names[MRDPLOT_L_FOOT_Z] = "l_foot_z";
  
  names[MRDPLOT_R_FOOT_X] = "r_foot_x";
  names[MRDPLOT_R_FOOT_Z] = "r_foot_z";
  
  names[MRDPLOT_L_FOOT_FX] = "l_foot_fx";
  names[MRDPLOT_L_FOOT_FZ] = "l_foot_fz";
  
  names[MRDPLOT_R_FOOT_FX] = "r_foot_fx";
  names[MRDPLOT_R_FOOT_FZ] = "r_foot_fz";
  names[MRDPLOT_COP_X] = "cop_x";
  names[MRDPLOT_L_FOOT_COP_X] = "l_foot_cop_x";
  names[MRDPLOT_R_FOOT_COP_X] = "r_foot_cop_x";
  names[MRDPLOT_COF_X] = "cof_x";
  
  names[MRDPLOT_CSTATE] = "cstate";
  names[MRDPLOT_CSTATE_ELAPSED_TIME] = "cstate_etime";
  names[MRDPLOT_LAST_IMPULSE_SIDE] = "last_impulse_side";
  names[MRDPLOT_LAST_IMPULSE_X] = "last_impulse_x";
  names[MRDPLOT_LAST_IMPULSE_Z] = "last_impulse_z";
  
  names[MRDPLOT_STATUS] = "status";
  names[MRDPLOT_SDFAST_MODEL] = "sdfast_model";
  names[MRDPLOT_SS_FOOT_DOWN] = "ss_foot_down";
  names[MRDPLOT_L_FOOT_STATUS] = "l_foot_status";
  names[MRDPLOT_R_FOOT_STATUS] = "r_foot_status";

  names[MRDPLOT_TORSO_ABS_ANGLE] = "torso_abs_angle";
  
  names[MRDPLOT_L_THIGH_ABS_ANGLE] = "l_thigh_abs_angle";
  names[MRDPLOT_L_CALF_ABS_ANGLE] = "l_calf_abs_angle";
  
  names[MRDPLOT_R_THIGH_ABS_ANGLE] = "r_thigh_abs_angle";
  names[MRDPLOT_R_CALF_ABS_ANGLE] = "r_calf_abs_angle";
  
  names[MRDPLOT_L_HIP_COMMAND] = "l_hip_command";
  names[MRDPLOT_L_KNEE_COMMAND] = "l_knee_command";
  names[MRDPLOT_L_ANKLE_COMMAND] = "l_ankle_command";

  names[MRDPLOT_R_HIP_COMMAND] = "r_hip_command";
  names[MRDPLOT_R_KNEE_COMMAND] = "r_knee_command";
  names[MRDPLOT_R_ANKLE_COMMAND] = "r_ankle_command";

  names[MRDPLOT_ENERGY] = "energy";
  names[MRDPLOT_PREDICTED_X] = "predicted_x";

  names[MRDPLOT_SDFAST_STATE0] = "sdfast_state0";
  names[MRDPLOT_SDFAST_STATE1] = "sdfast_state1";
  names[MRDPLOT_SDFAST_STATE2] = "sdfast_state2";
  names[MRDPLOT_SDFAST_STATE3] = "sdfast_state3";
  names[MRDPLOT_SDFAST_STATE4] = "sdfast_state4";
  names[MRDPLOT_SDFAST_STATE5] = "sdfast_state5";
  names[MRDPLOT_SDFAST_STATE6] = "sdfast_state6";
  names[MRDPLOT_SDFAST_STATE7] = "sdfast_state7";
  names[MRDPLOT_SDFAST_STATE8] = "sdfast_state8";
  names[MRDPLOT_SDFAST_STATE9] = "sdfast_state9";

  units[MRDPLOT_TIME] = "s";

  units[MRDPLOT_L_HIP] = "r";
  units[MRDPLOT_L_KNEE] = "r";
  units[MRDPLOT_L_ANKLE] = "r";
  
  units[MRDPLOT_R_HIP] = "r";
  units[MRDPLOT_R_KNEE] = "r";
  units[MRDPLOT_R_ANKLE] = "r";

  units[MRDPLOT_L_HIP_D] = "r";
  units[MRDPLOT_L_KNEE_D] = "r";
  units[MRDPLOT_L_ANKLE_D] = "r";
  
  units[MRDPLOT_R_HIP_D] = "r";
  units[MRDPLOT_R_KNEE_D] = "r";
  units[MRDPLOT_R_ANKLE_D] = "r";
  
  units[MRDPLOT_L_HIPD] = "r/s";
  units[MRDPLOT_L_KNEED] = "r/s";
  units[MRDPLOT_L_ANKLED] = "r/s";
  
  units[MRDPLOT_R_HIPD] = "r/s";
  units[MRDPLOT_R_KNEED] = "r/s";
  units[MRDPLOT_R_ANKLED] = "r/s";
  
  units[MRDPLOT_L_HIPD_D] = "r/s";
  units[MRDPLOT_L_KNEED_D] = "r/s";
  units[MRDPLOT_L_ANKLED_D] = "r/s";
  
  units[MRDPLOT_R_HIPD_D] = "r/s";
  units[MRDPLOT_R_KNEED_D] = "r/s";
  units[MRDPLOT_R_ANKLED_D] = "r/s";

  units[MRDPLOT_COM_X] = "m";
  units[MRDPLOT_COM_Z] = "m";
  units[MRDPLOT_COM_XD] = "m/s";
  units[MRDPLOT_COM_ZD] = "m/s";
  units[MRDPLOT_COM_TO_COP_X] = "m";
  units[MRDPLOT_COM_TO_COF_X] = "m";
  units[MRDPLOT_COM_ALT_X] = "m";

  units[MRDPLOT_X] = "m";
  units[MRDPLOT_Z] = "m";
  units[MRDPLOT_ROLL] = "r";
  
  units[MRDPLOT_XD] = "m/s";
  units[MRDPLOT_ZD] = "m/s";
  units[MRDPLOT_ROLLD] = "r/s";
  
  units[MRDPLOT_L_HIP_TORQUE] = "Nm";
  units[MRDPLOT_L_KNEE_FORCE] = "N";
  units[MRDPLOT_L_ANKLE_TORQUE] = "Nm";
  
  units[MRDPLOT_R_HIP_TORQUE] = "Nm";
  units[MRDPLOT_R_KNEE_FORCE] = "N";
  units[MRDPLOT_R_ANKLE_TORQUE] = "Nm";
  
  units[MRDPLOT_HEAD_X] = "m";
  units[MRDPLOT_HEAD_Z] = "m";
  units[MRDPLOT_GROIN_X] = "m";
  units[MRDPLOT_GROIN_Z] = "m";
  units[MRDPLOT_L_HIP_X] = "m";
  units[MRDPLOT_L_HIP_Z] = "m";
  units[MRDPLOT_R_HIP_X] = "m";
  units[MRDPLOT_R_HIP_Z] = "m";
  units[MRDPLOT_L_KNEE_X] = "m";
  units[MRDPLOT_L_KNEE_Z] = "m";

  units[MRDPLOT_R_KNEE_X] = "m";
  units[MRDPLOT_R_KNEE_Z] = "m";

  units[MRDPLOT_L_FOOT_X] = "m";
  units[MRDPLOT_L_FOOT_Z] = "m";
  
  units[MRDPLOT_R_FOOT_X] = "m";
  units[MRDPLOT_R_FOOT_Z] = "m";
  
  units[MRDPLOT_L_FOOT_FX] = "N";
  units[MRDPLOT_L_FOOT_FZ] = "N";
  
  units[MRDPLOT_R_FOOT_FX] = "N";
  units[MRDPLOT_R_FOOT_FZ] = "N";

  units[MRDPLOT_COP_X] = "m";
  units[MRDPLOT_L_FOOT_COP_X] = "m";
  units[MRDPLOT_R_FOOT_COP_X] = "m";
  units[MRDPLOT_COF_X] = "m";
  
  units[MRDPLOT_CSTATE] = "-";
  units[MRDPLOT_CSTATE_ELAPSED_TIME] = "s";
  units[MRDPLOT_LAST_IMPULSE_SIDE] = "-";
  units[MRDPLOT_LAST_IMPULSE_X] = "Ns";
  units[MRDPLOT_LAST_IMPULSE_Z] = "Ns";
  
  units[MRDPLOT_STATUS] = "-";
  units[MRDPLOT_SDFAST_MODEL] = "-";
  units[MRDPLOT_SS_FOOT_DOWN] = "-";
  units[MRDPLOT_L_FOOT_STATUS] = "-";
  units[MRDPLOT_R_FOOT_STATUS] = "-";

  units[MRDPLOT_TORSO_ABS_ANGLE] = "r";
  
  units[MRDPLOT_L_THIGH_ABS_ANGLE] = "r";
  units[MRDPLOT_L_CALF_ABS_ANGLE] = "r";
  
  units[MRDPLOT_R_THIGH_ABS_ANGLE] = "r";
  units[MRDPLOT_R_CALF_ABS_ANGLE] = "r";
  
  units[MRDPLOT_L_HIP_COMMAND] = "Nm";
  units[MRDPLOT_L_KNEE_COMMAND] = "Nm";
  units[MRDPLOT_L_ANKLE_COMMAND] = "Nm";

  units[MRDPLOT_R_HIP_COMMAND] = "Nm";
  units[MRDPLOT_R_KNEE_COMMAND] = "Nm";
  units[MRDPLOT_R_ANKLE_COMMAND] = "Nm";

  units[MRDPLOT_ENERGY] = "-";
  units[MRDPLOT_PREDICTED_X] = "-";
  
  units[MRDPLOT_SDFAST_STATE0] = "-";
  units[MRDPLOT_SDFAST_STATE1] = "-";
  units[MRDPLOT_SDFAST_STATE2] = "-";
  units[MRDPLOT_SDFAST_STATE3] = "-";
  units[MRDPLOT_SDFAST_STATE4] = "-";
  units[MRDPLOT_SDFAST_STATE5] = "-";
  units[MRDPLOT_SDFAST_STATE6] = "-";
  units[MRDPLOT_SDFAST_STATE7] = "-";
  units[MRDPLOT_SDFAST_STATE8] = "-";
  units[MRDPLOT_SDFAST_STATE9] = "-";

  /*
  for( i = 0; i < N_CHANNELS; i++ )
    {
      printf( "%d %s %s\n", i, names[i], units[i] );
    }
  */
}

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

int save_data( SIM *s )
{
  static int myindex = 0;

  if ( n_points >= MAX_N_POINTS )
    return 0;

  data[myindex + MRDPLOT_TIME] = s->time;

  data[myindex + MRDPLOT_L_HIP] = s->hip_angle[LEFT];
  data[myindex + MRDPLOT_L_KNEE] = s->knee_length[LEFT];
  data[myindex + MRDPLOT_L_ANKLE] = s->ankle_angle[LEFT];

  data[myindex + MRDPLOT_R_HIP] = s->hip_angle[RIGHT];
  data[myindex + MRDPLOT_R_KNEE] = s->knee_length[RIGHT];
  data[myindex + MRDPLOT_R_ANKLE] = s->ankle_angle[RIGHT];

  data[myindex + MRDPLOT_L_HIP_D] = s->hip_angle_d[LEFT];
  data[myindex + MRDPLOT_L_KNEE_D] = s->knee_length_d[LEFT];
  data[myindex + MRDPLOT_L_ANKLE_D] = s->ankle_angle_d[LEFT];

  data[myindex + MRDPLOT_R_HIP_D] = s->hip_angle_d[RIGHT];
  data[myindex + MRDPLOT_R_KNEE_D] = s->knee_length_d[RIGHT];
  data[myindex + MRDPLOT_R_ANKLE_D] = s->ankle_angle_d[RIGHT];

  data[myindex + MRDPLOT_L_HIPD] = s->hip_angled[LEFT];
  data[myindex + MRDPLOT_L_KNEED] = s->knee_lengthd[LEFT];
  data[myindex + MRDPLOT_L_ANKLED] = s->ankle_angled[LEFT];

  data[myindex + MRDPLOT_R_HIPD] = s->hip_angled[RIGHT];
  data[myindex + MRDPLOT_R_KNEED] = s->knee_lengthd[RIGHT];
  data[myindex + MRDPLOT_R_ANKLED] = s->ankle_angled[RIGHT];

  data[myindex + MRDPLOT_L_HIPD_D] = s->hip_angled_d[LEFT];
  data[myindex + MRDPLOT_L_KNEED_D] = s->knee_lengthd_d[LEFT];
  data[myindex + MRDPLOT_L_ANKLED_D] = s->ankle_angled_d[LEFT];

  data[myindex + MRDPLOT_R_HIPD_D] = s->hip_angled_d[RIGHT];
  data[myindex + MRDPLOT_R_KNEED_D] = s->knee_lengthd_d[RIGHT];
  data[myindex + MRDPLOT_R_ANKLED_D] = s->ankle_angled_d[RIGHT];

  data[myindex + MRDPLOT_COM_X] = s->com[XX];
  data[myindex + MRDPLOT_COM_Z] = s->com[ZZ];
  data[myindex + MRDPLOT_COM_XD] = s->comd[XX];
  data[myindex + MRDPLOT_COM_ZD] = s->comd[ZZ];
  data[myindex + MRDPLOT_COM_TO_COP_X] = s->com_to_cop[XX];
  data[myindex + MRDPLOT_COM_TO_COF_X] = s->com_to_cof[XX];
  data[myindex + MRDPLOT_COM_ALT_X] = s->com_alt[XX];

  data[myindex + MRDPLOT_X] = (s->hip[LEFT][XX] + s->hip[RIGHT][XX]);
  data[myindex + MRDPLOT_Z] = (s->hip[LEFT][ZZ] + s->hip[RIGHT][ZZ]);
  data[myindex + MRDPLOT_ROLL] = s->roll;

  data[myindex + MRDPLOT_XD] = (s->hipd[LEFT][XX] + s->hipd[RIGHT][XX]);
  data[myindex + MRDPLOT_ZD] = (s->hipd[LEFT][ZZ] + s->hipd[RIGHT][ZZ]);
  data[myindex + MRDPLOT_ROLLD] = s->rolld;

  data[myindex + MRDPLOT_L_HIP_TORQUE] = s->hip_torque[LEFT];
  data[myindex + MRDPLOT_L_KNEE_FORCE] = s->knee_force[LEFT];
  data[myindex + MRDPLOT_L_ANKLE_TORQUE] = s->ankle_torque[LEFT];

  data[myindex + MRDPLOT_R_HIP_TORQUE] = s->hip_torque[RIGHT];
  data[myindex + MRDPLOT_R_KNEE_FORCE] = s->knee_force[RIGHT];
  data[myindex + MRDPLOT_R_ANKLE_TORQUE] = s->ankle_torque[RIGHT];

  data[myindex + MRDPLOT_HEAD_X] = s->head[XX];
  data[myindex + MRDPLOT_HEAD_Z] = s->head[ZZ];
  data[myindex + MRDPLOT_GROIN_X] = s->groin[XX];
  data[myindex + MRDPLOT_GROIN_Z] = s->groin[ZZ];
  data[myindex + MRDPLOT_L_HIP_X] = s->hip[LEFT][XX];
  data[myindex + MRDPLOT_L_HIP_Z] = s->hip[LEFT][ZZ];
  data[myindex + MRDPLOT_R_HIP_X] = s->hip[RIGHT][XX];
  data[myindex + MRDPLOT_R_HIP_Z] = s->hip[RIGHT][ZZ];
  data[myindex + MRDPLOT_L_KNEE_X] = s->knee[LEFT][XX];
  data[myindex + MRDPLOT_L_KNEE_Z] = s->knee[LEFT][ZZ];
  data[myindex + MRDPLOT_R_KNEE_X] = s->knee[RIGHT][XX];
  data[myindex + MRDPLOT_R_KNEE_Z] = s->knee[RIGHT][ZZ];

  data[myindex + MRDPLOT_L_FOOT_X] = s->foot[LEFT][XX];
  data[myindex + MRDPLOT_L_FOOT_Z] = s->foot[LEFT][ZZ];

  data[myindex + MRDPLOT_R_FOOT_X] = s->foot[RIGHT][XX];
  data[myindex + MRDPLOT_R_FOOT_Z] = s->foot[RIGHT][ZZ];

  data[myindex + MRDPLOT_L_FOOT_FX] = s->ground_force[LEFT][XX];
  data[myindex + MRDPLOT_L_FOOT_FZ] = s->ground_force[LEFT][ZZ];

  data[myindex + MRDPLOT_R_FOOT_FX] = s->ground_force[RIGHT][XX];
  data[myindex + MRDPLOT_R_FOOT_FZ] = s->ground_force[RIGHT][ZZ];

  data[myindex + MRDPLOT_COP_X] = s->cop[XX];
  data[myindex + MRDPLOT_L_FOOT_COP_X] = s->cop_foot[LEFT][XX];
  data[myindex + MRDPLOT_R_FOOT_COP_X] = s->cop_foot[RIGHT][XX];
  data[myindex + MRDPLOT_COF_X] = s->cof[XX];

  data[myindex + MRDPLOT_CSTATE] = s->controller_state;
  data[myindex + MRDPLOT_CSTATE_ELAPSED_TIME] = s->cstate_elapsed_time;

  data[myindex + MRDPLOT_LAST_IMPULSE_SIDE] = s->last_impulse_side;
  data[myindex + MRDPLOT_LAST_IMPULSE_X] = s->last_impulse[XX];
  data[myindex + MRDPLOT_LAST_IMPULSE_Z] = s->last_impulse[ZZ];

  data[myindex + MRDPLOT_STATUS] = s->status;
  data[myindex + MRDPLOT_SDFAST_MODEL] = s->sdfast_model;
  data[myindex + MRDPLOT_SS_FOOT_DOWN] = s->ss_foot_down;
  data[myindex + MRDPLOT_L_FOOT_STATUS] = s->foot_status[LEFT];
  data[myindex + MRDPLOT_R_FOOT_STATUS] = s->foot_status[RIGHT];

  data[myindex + MRDPLOT_TORSO_ABS_ANGLE] = s->torso_abs_angle;

  data[myindex + MRDPLOT_L_THIGH_ABS_ANGLE] = s->thigh_abs_angle[LEFT];
  data[myindex + MRDPLOT_L_CALF_ABS_ANGLE] = s->calf_abs_angle[LEFT];

  data[myindex + MRDPLOT_R_THIGH_ABS_ANGLE] = s->thigh_abs_angle[RIGHT];
  data[myindex + MRDPLOT_R_CALF_ABS_ANGLE] = s->calf_abs_angle[RIGHT];

  data[myindex + MRDPLOT_L_HIP_COMMAND] = s->hip_command[LEFT];
  data[myindex + MRDPLOT_L_KNEE_COMMAND] = s->knee_command[LEFT];
  data[myindex + MRDPLOT_L_ANKLE_COMMAND] = s->ankle_command[LEFT];

  data[myindex + MRDPLOT_R_HIP_COMMAND] = s->hip_command[RIGHT];
  data[myindex + MRDPLOT_R_KNEE_COMMAND] = s->knee_command[RIGHT];
  data[myindex + MRDPLOT_R_ANKLE_COMMAND] = s->ankle_command[RIGHT];

  data[myindex + MRDPLOT_ENERGY] = s->energy;
  data[myindex + MRDPLOT_PREDICTED_X] = s->predicted_x;

  data[myindex + MRDPLOT_SDFAST_STATE0] = s->sdfast_state[0];
  data[myindex + MRDPLOT_SDFAST_STATE1] = s->sdfast_state[1];
  data[myindex + MRDPLOT_SDFAST_STATE2] = s->sdfast_state[2];
  data[myindex + MRDPLOT_SDFAST_STATE3] = s->sdfast_state[3];
  data[myindex + MRDPLOT_SDFAST_STATE4] = s->sdfast_state[4];
  data[myindex + MRDPLOT_SDFAST_STATE5] = s->sdfast_state[5];
  data[myindex + MRDPLOT_SDFAST_STATE6] = s->sdfast_state[6];
  data[myindex + MRDPLOT_SDFAST_STATE7] = s->sdfast_state[7];
  data[myindex + MRDPLOT_SDFAST_STATE8] = s->sdfast_state[8];
  data[myindex + MRDPLOT_SDFAST_STATE9] = s->sdfast_state[9];

  // printf( "%g %g\n", data[myindex + MRDPLOT_COM_ALT_X], s->com_alt[XX] );

  myindex += N_CHANNELS;
  n_points++;

  return 0;
}

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

int write_matlab_file( char *filename )
{
  FILE *stream;
  int i;
  static int myindex = 0;

  stream = fopen( filename, "w" );
  if ( stream == NULL )
    {
      fprintf( stderr, "Can't open %s for write.\n", filename );
      exit( -1 );
    }
  for ( i = 0; i < n_points; i++ )
    {
      fprintf( stream, "%g ", data[myindex + MRDPLOT_TIME] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_ANKLE] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_KNEE] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_HIP] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_ANKLED] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_KNEED] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_HIPD] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_ANKLE_TORQUE] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_KNEE_FORCE] );
      fprintf( stream, "%g ", data[myindex + MRDPLOT_L_HIP_TORQUE] );
      fprintf( stream, "\n" );
      myindex += N_CHANNELS;
    }
  fclose( stream );

  return 0;
}

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

int write_the_mrdplot_file( SIM *s )
{
  MRDPLOT_DATA *d; 

  d = malloc_mrdplot_data( 0, 0 );
  d->filename = generate_file_name();
  d->n_channels = N_CHANNELS;
  d->n_points = n_points;
  d->total_n_numbers = d->n_channels*d->n_points;
  d->frequency = 1/s->time_step;
  d->data = data;
  d->names = names;
  d->units = units;

  write_mrdplot_file( d );

  // write_matlab_file( "dd" );

  return 0;
}

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