#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "dynamics-leg.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 CALF_LENGTH (0.5)	// length of link
#define CALF_MASS (0.5)     // mass of link
#define TORSOTOHIP  -0.5
#define HIPTOTHIGHCOM -0.25
#define HIPTOKNEE  -0.5
#define KNEETOCALFCOM -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 Icalf = (CALF_MASS*SQ(THIGH_LENGTH)/12); /* Icom */
  double c0, s0, c0h, s0h, c0hk, s0hk;
  double thigh_xd_rel, thigh_yd_rel;
  double calf_xd_rel, calf_yd_rel;
  double thigh_xd, thigh_yd;
  double calf_xd, calf_yd;
  double l1 = TORSOTOHIP;
  double l2cm = HIPTOTHIGHCOM;
  double l2 = HIPTOKNEE;
  double l3cm = KNEETOCALFCOM;
  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.375;
  state[S_YD] = 0.0;
  state[S_A0D] = 1.0;
  state[S_HIPLD] = -0.5;
  state[S_KNEELD] = 0.25;
  action[A_HIPL] = 0;
  action[A_KNEEL] = 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] );
      c0hk = cos( d->state[S_A0] + d->state[S_HIPL] + d->state[S_KNEEL] );
      s0hk = sin( d->state[S_A0] + d->state[S_HIPL] + d->state[S_KNEEL] );

      thigh_xd_rel = -l1*c0*d->state[S_A0D] 
	- l2cm*c0h*(d->state[S_A0D] + d->state[S_HIPLD]);
      thigh_yd_rel = -l1*s0*d->state[S_A0D] 
	- l2cm*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;

      calf_xd_rel = -l1*c0*d->state[S_A0D] 
	- l2*c0h*(d->state[S_A0D] + d->state[S_HIPLD])
	- l3cm*c0hk*(d->state[S_A0D] + d->state[S_HIPLD] + d->state[S_KNEELD]);
      calf_yd_rel = -l1*s0*d->state[S_A0D] 
	- l2*s0h*(d->state[S_A0D] + d->state[S_HIPLD])
	- l3cm*s0hk*(d->state[S_A0D] + d->state[S_HIPLD] + d->state[S_KNEELD]);
      calf_xd = d->state[S_XD] + calf_xd_rel;
      calf_yd = d->state[S_YD] + calf_yd_rel;

      x_com = d->state[S_X] 
	+ (THIGH_MASS*(-l1*s0 - l2cm*s0h) + CALF_MASS*(-l1*s0 - l2*s0h - l3cm*s0hk))
	/(TORSO_MASS + THIGH_MASS + CALF_MASS);
      y_com = d->state[S_Y] 
	+ (THIGH_MASS*(l1*c0 + l2cm*c0h) + CALF_MASS*(l1*c0 + l2*c0h + l3cm*c0hk))
	/(TORSO_MASS + THIGH_MASS + CALF_MASS);

      x_comd = d->state[S_XD] 
	+ (THIGH_MASS*thigh_xd_rel + CALF_MASS*calf_xd_rel)
	/(TORSO_MASS + THIGH_MASS + CALF_MASS);
      y_comd = d->state[S_YD] 
	+ (THIGH_MASS*thigh_yd_rel + CALF_MASS*calf_yd_rel)
	/(TORSO_MASS + THIGH_MASS + CALF_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])
	+ 0.5*CALF_MASS*(SQ(calf_xd) + SQ(calf_yd))
	+ 0.5*Icalf*SQ(d->state[S_A0D] + d->state[S_HIPLD] + d->state[S_KNEELD]);
      if ( i % 2000 == 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 %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_KNEEL],
		d->state[S_XD], d->state[S_YD],
		d->state[S_A0D], d->state[S_HIPLD], d->state[S_KNEELD],
		x_com, y_com, x_comd, y_comd,
		energy );
      integrate_one_step( d, action, NULL );
    }
}