#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "dynamics.h"

#define TORSO_LENGTH (1.0)	// length of link
#define TORSO_MASS (1.0)     // mass of link
#define THIGH_LENGTH (0.5)	// length of link
#define THIGH_MASS (0.5)     // mass of link
#define TORSOTOHIP  -0.5
#define HIPTOTHIGHCOM -0.25

/* Handy macros */
#define SQ(x) ((x)*(x))

main()
{
  int i;
  Dynamics *d;
  double state[N_STATE_DIMENSIONS];
  double action[N_ACTION_DIMENSIONS];
  double energy;
  double Itorso = (TORSO_MASS*SQ(TORSO_LENGTH)/12); /* Icom */
  double Ithigh = (THIGH_MASS*SQ(THIGH_LENGTH)/12); /* Icom */
  double c0, s0, c0h, s0h;
  double thigh_xd_rel, thigh_yd_rel;
  double thigh_xd, thigh_yd;
  double l1 = TORSOTOHIP;
  double l2 = HIPTOTHIGHCOM;
  double x_com, y_com;
  double x_comd, y_comd;

  init_dynamics();
  d = create_dynamics();

  for( i = 0; i < N_STATE_DIMENSIONS; i++ )
    state[i] = 0;

  state[S_XD] = -0.240;
  state[S_YD] = 0.040;
  state[S_A0D] = 1.0;
  state[S_HIPL] = -0.5;
  action[A_HIPL] = 0;

  set_state( d, 0.0, state );

  for( i = 0; d->time < 10.0; i++ )
    {
      c0 = cos( d->state[S_A0] );
      s0 = sin( d->state[S_A0] );
      c0h = cos( d->state[S_A0] + d->state[S_HIPL] );
      s0h = sin( d->state[S_A0] + d->state[S_HIPL] );

      thigh_xd_rel = -l1*c0*d->state[S_A0D] 
	- l2*c0h*(d->state[S_A0D] + d->state[S_HIPLD]);
      thigh_yd_rel = -l1*s0*d->state[S_A0D] 
	- l2*s0h*(d->state[S_A0D] + d->state[S_HIPLD]);
      thigh_xd = d->state[S_XD] + thigh_xd_rel;
      thigh_yd = d->state[S_YD] + thigh_yd_rel;
      x_com = d->state[S_X] + THIGH_MASS*(-l1*s0 - l2*s0h)/(TORSO_MASS + THIGH_MASS);
      y_com = d->state[S_Y] + THIGH_MASS*(l1*c0 + l2*c0h)/(TORSO_MASS + THIGH_MASS);
      x_comd = d->state[S_XD] + THIGH_MASS*thigh_xd_rel/(TORSO_MASS + THIGH_MASS);
      y_comd = d->state[S_YD] + THIGH_MASS*thigh_yd_rel/(TORSO_MASS + THIGH_MASS);
      energy = 0.5*TORSO_MASS*(SQ(d->state[S_XD]) + SQ(d->state[S_YD]))
	+ 0.5*Itorso*SQ(d->state[S_A0D])
	+ 0.5*THIGH_MASS*(SQ(thigh_xd) + SQ(thigh_yd))
	+ 0.5*Ithigh*SQ(d->state[S_A0D] + d->state[S_HIPLD]);
      if ( i % 10 == 0 )
	printf( "%10.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %g\n", 
		d->time,
		d->state[S_X], d->state[S_Y],
		d->state[S_A0], d->state[S_HIPL],
		d->state[S_XD], d->state[S_YD],
		d->state[S_A0D], d->state[S_HIPLD],
		x_com, y_com, x_comd, y_comd,
		energy );
      integrate_one_step( d, action, NULL );
    }
}