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

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

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

/* The idea is to keep track of each terms dependence on joint acceleration. */

/* number of joints plus 1 */
#define NJP1 5
#define CONST 0
#define A1DD 1
#define A2DD 2
#define A3DD 3
#define A4DD 4

/************************************************************************/
/* Parameters */

#define LENGTH (1.0/4.0)	// length of link
#define WIDTH (0.1)    // width of link
#define MASS (1.0/4.0)     // mass of link

#define GRAVITY 9.81
// #define VISCOUS_FRICTION 0.1 // viscous friction at each joint
#define VISCOUS_FRICTION 0.0 // viscous friction at each joint

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

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

int seed = 2;

float randomf( float min, float max )
{
  return ((float) rand_r( &(seed) )/ (float) RAND_MAX)*(max - min) + min;
}

float randomd( double min, double max )
{
  return ((double) rand_r( &(seed) )/ (double) RAND_MAX)*(max - min) + min;
}

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

int scalar_times( double s, double *src, double *dst )
{
  int i;

  for( i = 0; i < NJP1; i++ )
    dst[i] = s*src[i];
}

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

int plus( double *src, double *dst )
{
  int i;

  for( i = 0; i < NJP1; i++ )
    dst[i] += src[i];
}

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

int copy( double *src, double *dst )
{
  int i;

  for( i = 0; i < NJP1; i++ )
    dst[i] = src[i];
}

/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/* 4 microseconds */

forward_dynamics( double a1, double a2, double a3, double a4,
		  double a1d, double a2d, double a3d, double a4d,
		  double t1, double t2, double t3, double t4,
		  double *a1dd, double *a2dd, double *a3dd, double *a4dd )
{
  double m1 = MASS;
  double m2 = MASS;
  double m3 = MASS;
  double m4 = MASS;
  double l1cm = (LENGTH/2);
  double l2cm = (LENGTH/2);
  double l3cm = (LENGTH/2);
  double l4cm = (LENGTH/2);
  double l1 = LENGTH;
  double l2 = LENGTH;
  double l3 = LENGTH;
  double G = GRAVITY;
  double I1 = (MASS*(LENGTH*LENGTH + WIDTH*WIDTH)/12);
  double I2 = (MASS*(LENGTH*LENGTH + WIDTH*WIDTH)/12);
  double I3 = (MASS*(LENGTH*LENGTH + WIDTH*WIDTH)/12);
  double I4 = (MASS*(LENGTH*LENGTH + WIDTH*WIDTH)/12);
  double a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p;
  double q, r, s, t;
  double determinant;
  double s1, c1, s12, c12, s123, c123, s1234, c1234;
  double p2dd_x[NJP1];
  double p2dd_y[NJP1];
  double p3dd_x[NJP1];
  double p3dd_y[NJP1];
  double p4dd_x[NJP1];
  double p4dd_y[NJP1];
  double q1dd_x[NJP1];
  double q1dd_y[NJP1];
  double q2dd_x[NJP1];
  double q2dd_y[NJP1];
  double q3dd_x[NJP1];
  double q3dd_y[NJP1];
  double q4dd_x[NJP1];
  double q4dd_y[NJP1];
  double f1_x[NJP1];
  double f1_y[NJP1];
  double f2_x[NJP1];
  double f2_y[NJP1];
  double f3_x[NJP1];
  double f3_y[NJP1];
  double f4_x[NJP1];
  double f4_y[NJP1];
  double f12_x[NJP1];
  double f12_y[NJP1];
  double f23_x[NJP1];
  double f23_y[NJP1];
  double f34_x[NJP1];
  double f34_y[NJP1];
  double n1[NJP1];
  double n2[NJP1];
  double n3[NJP1];
  double n4[NJP1];
  double tmp[NJP1];
  double l1v_x, l1v_y;
  double l2v_x, l2v_y;
  double l3v_x, l3v_y;
  double cm1_x, cm1_y;
  double cm2_x, cm2_y;
  double cm3_x, cm3_y;
  double cm4_x, cm4_y;
  double a1d_2, a12d_2, a123d_2, a1234d_2;

  s1 = sin( a1 );
  c1 = cos( a1 );
  s12 = sin( a1 + a2 );
  c12 = cos( a1 + a2 );
  s123 = sin( a1 + a2 + a3 );
  c123 = cos( a1 + a2 + a3 );
  s1234 = sin( a1 + a2 + a3 + a4 );
  c1234 = cos( a1 + a2 + a3 + a4 );

  a1d_2 = a1d*a1d;

  cm1_x = l1cm*s1;
  cm1_y = -l1cm*c1;

  q1dd_x[CONST] = -a1d_2*cm1_x;
  q1dd_x[A1DD] = -cm1_y;
  q1dd_x[A2DD] = 0;
  q1dd_x[A3DD] = 0;
  q1dd_x[A4DD] = 0;
  q1dd_y[CONST] = -a1d_2*cm1_y;
  q1dd_y[A1DD] = cm1_x;
  q1dd_y[A2DD] = 0;
  q1dd_y[A3DD] = 0;
  q1dd_y[A4DD] = 0;

  l1v_x = l1*s1;
  l1v_y = -l1*c1;

  p2dd_x[CONST] = -a1d_2*l1v_x;
  p2dd_x[A1DD] = -l1v_y;
  p2dd_x[A2DD] = 0;
  p2dd_x[A3DD] = 0;
  p2dd_x[A4DD] = 0;
  p2dd_y[CONST] = -a1d_2*l1v_y;
  p2dd_y[A1DD] = l1v_x;
  p2dd_y[A2DD] = 0;
  p2dd_y[A3DD] = 0;
  p2dd_y[A4DD] = 0;

  a12d_2 = a1d + a2d;
  a12d_2 = a12d_2*a12d_2;

  cm2_x = l2cm*s12;
  cm2_y = -l2cm*c12;

  q2dd_x[CONST] = -a12d_2*cm2_x + p2dd_x[CONST];
  q2dd_x[A1DD] = -cm2_y + p2dd_x[A1DD];
  q2dd_x[A2DD] = -cm2_y;
  q2dd_x[A3DD] = 0.0;
  q2dd_x[A4DD] = 0.0;
  q2dd_y[CONST] = -a12d_2*cm2_y + p2dd_y[CONST];
  q2dd_y[A1DD] = cm2_x + p2dd_y[A1DD];
  q2dd_y[A2DD] = cm2_x;
  q2dd_y[A3DD] = 0.0;
  q2dd_y[A4DD] = 0.0;

  l2v_x = l2*s12;
  l2v_y = -l2*c12;

  p3dd_x[CONST] = -a12d_2*l2v_x + p2dd_x[CONST];
  p3dd_x[A1DD] = -l2v_y + p2dd_x[A1DD];
  p3dd_x[A2DD] = -l2v_y;
  p3dd_x[A3DD] = 0;
  p3dd_x[A4DD] = 0;
  p3dd_y[CONST] = -a12d_2*l2v_y + p2dd_y[CONST];
  p3dd_y[A1DD] = l2v_x + p2dd_y[A1DD];
  p3dd_y[A2DD] = l2v_x;
  p3dd_y[A3DD] = 0;
  p3dd_y[A4DD] = 0;

  a123d_2 = a1d + a2d + a3d;
  a123d_2 = a123d_2*a123d_2;

  cm3_x = l3cm*s123;
  cm3_y = -l3cm*c123;

  q3dd_x[CONST] = -a123d_2*cm3_x + p3dd_x[CONST];
  q3dd_x[A1DD] = -cm3_y + p3dd_x[A1DD]; 
  q3dd_x[A2DD] = -cm3_y + p3dd_x[A2DD];
  q3dd_x[A3DD] = -cm3_y;
  q3dd_x[A4DD] = 0;
  q3dd_y[CONST] = -a123d_2*cm3_y + p3dd_y[CONST];
  q3dd_y[A1DD] = cm3_x + p3dd_y[A1DD];
  q3dd_y[A2DD] = cm3_x + p3dd_y[A2DD];
  q3dd_y[A3DD] = cm3_x;
  q3dd_y[A4DD] = 0;

  /*************************************************************/
  l3v_x = l3*s123;
  l3v_y = -l3*c123;

  p4dd_x[CONST] = -a123d_2*l3v_x + p3dd_x[CONST];
  p4dd_x[A1DD] = -l3v_y + p3dd_x[A1DD];
  p4dd_x[A2DD] = -l3v_y + p3dd_x[A2DD];
  p4dd_x[A3DD] = -l3v_y;
  p4dd_x[A4DD] = 0;
  p4dd_y[CONST] = -a123d_2*l3v_y + p3dd_y[CONST];
  p4dd_y[A1DD] = l3v_x + p3dd_y[A1DD];
  p4dd_y[A2DD] = l3v_x + p3dd_y[A2DD];
  p4dd_y[A3DD] = l3v_x;
  p4dd_y[A4DD] = 0;

  a1234d_2 = a1d + a2d + a3d + a4d;
  a1234d_2 = a1234d_2*a1234d_2;

  cm4_x = l4cm*s1234;
  cm4_y = -l4cm*c1234;

  q4dd_x[CONST] = -a1234d_2*cm4_x + p4dd_x[CONST];
  q4dd_x[A1DD] = -cm4_y + p4dd_x[A1DD]; 
  q4dd_x[A2DD] = -cm4_y + p4dd_x[A2DD];
  q4dd_x[A3DD] = -cm4_y + p4dd_x[A3DD];
  q4dd_x[A4DD] = -cm4_y;
  q4dd_y[CONST] = -a1234d_2*cm4_y + p4dd_y[CONST];
  q4dd_y[A1DD] = cm4_x + p4dd_y[A1DD];
  q4dd_y[A2DD] = cm4_x + p4dd_y[A2DD];
  q4dd_y[A3DD] = cm4_x + p4dd_y[A3DD];
  q4dd_y[A4DD] = cm4_x;

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

  scalar_times( m4, &(q4dd_x[0]), &(f4_x[0]) );
  scalar_times( m4, &(q4dd_y[0]), &(f4_y[0]) );
  f4_y[CONST] += m4*G;

  scalar_times( m3, &(q3dd_x[0]), &(f3_x[0]) );
  scalar_times( m3, &(q3dd_y[0]), &(f3_y[0]) );
  f3_y[CONST] += m3*G;

  scalar_times( m2, &(q2dd_x[0]), &(f2_x[0]) );
  scalar_times( m2, &(q2dd_y[0]), &(f2_y[0]) );
  f2_y[CONST] += m2*G;

  scalar_times( m1, &(q1dd_x[0]), &(f1_x[0]) );
  scalar_times( m1, &(q1dd_y[0]), &(f1_y[0]) );
  f1_y[CONST] += m1*G;

  copy( &(f4_x[0]), &(f34_x[0]) );
  copy( &(f4_y[0]), &(f34_y[0]) );

  copy( &(f34_x[0]), &(f23_x[0]) );
  copy( &(f34_y[0]), &(f23_y[0]) );
  plus( &(f3_x[0]), &(f23_x[0]) );
  plus( &(f3_y[0]), &(f23_y[0]) );

  copy( &(f23_x[0]), &(f12_x[0]) );
  copy( &(f23_y[0]), &(f12_y[0]) );
  plus( &(f2_x[0]), &(f12_x[0]) );
  plus( &(f2_y[0]), &(f12_y[0]) );

  n1[CONST] = 0;
  n1[A1DD] = I1;
  n1[A2DD] = 0;
  n1[A3DD] = 0;
  n1[A4DD] = 0;
  n2[CONST] = 0;
  n2[A1DD] = I2;
  n2[A2DD] = I2;
  n2[A3DD] = 0;
  n2[A4DD] = 0;
  n3[CONST] = 0;
  n3[A1DD] = I3;
  n3[A2DD] = I3;
  n3[A3DD] = I3;
  n3[A4DD] = 0;
  n4[CONST] = 0;
  n4[A1DD] = I4;
  n4[A2DD] = I4;
  n4[A3DD] = I4;
  n4[A4DD] = I4;

  /* Compute torques: n_i now has different meaning */

  /* Torque 4 (in n4) */
  scalar_times( cm4_x, &(f4_y[0]), tmp );
  plus( tmp, n4 );
  scalar_times( -cm4_y, &(f4_x[0]), tmp );
  plus( tmp, n4 );

  /* Torque 3 (in n3) */
  plus( n4, n3 );
  scalar_times( cm3_x, &(f3_y[0]), tmp );
  plus( tmp, n3 );
  scalar_times( -cm3_y, &(f3_x[0]), tmp );
  plus( tmp, n3 );
  scalar_times( l3v_x, &(f34_y[0]), tmp );
  plus( tmp, n3 );
  scalar_times( -l3v_y, &(f34_x[0]), tmp );
  plus( tmp, n3 );

  /* Torque 2 (in n2) */
  plus( n3, n2 );
  scalar_times( cm2_x, &(f2_y[0]), tmp );
  plus( tmp, n2 );
  scalar_times( -cm2_y, &(f2_x[0]), tmp );
  plus( tmp, n2 );
  scalar_times( l2v_x, &(f23_y[0]), tmp );
  plus( tmp, n2 );
  scalar_times( -l2v_y, &(f23_x[0]), tmp );
  plus( tmp, n2 );

  /* Torque 1 (in n1) */
  plus( n2, n1 );
  scalar_times( cm1_x, &(f1_y[0]), tmp );
  plus( tmp, n1 );
  scalar_times( -cm1_y, &(f1_x[0]), tmp );
  plus( tmp, n1 );
  scalar_times( l1v_x, &(f12_y[0]), tmp );
  plus( tmp, n1 );
  scalar_times( -l1v_y, &(f12_x[0]), tmp );
  plus( tmp, n1 );

  a = n1[A1DD];
  b = n1[A2DD];
  c = n1[A3DD];
  d = n1[A4DD];
  q = t1 - VISCOUS_FRICTION*a1d - n1[CONST];
  
  e = n2[A1DD];
  f = n2[A2DD];
  g = n2[A3DD];
  h = n2[A4DD];
  r = t2 - VISCOUS_FRICTION*a2d - n2[CONST];

  i = n3[A1DD];
  j = n3[A2DD];
  k = n3[A3DD];
  l = n3[A4DD];
  s = t3 - VISCOUS_FRICTION*a3d - n3[CONST];

  m = n4[A1DD];
  n = n4[A2DD];
  o = n4[A3DD];
  p = n4[A4DD];
  t = t4 - VISCOUS_FRICTION*a4d - n4[CONST];

  determinant =
    (d*g*j*m - c*h*j*m - d*f*k*m + b*h*k*m + c*f*l*m - b*g*l*m - d*g*i*n +
     c*h*i*n + d*e*k*n - a*h*k*n - c*e*l*n + a*g*l*n + d*f*i*o - b*h*i*o -
     d*e*j*o + a*h*j*o + b*e*l*o - a*f*l*o - c*f*i*p + b*g*i*p + c*e*j*p -
     a*g*j*p - b*e*k*p + a*f*k*p);
  *a1dd = q*(-(h*k*n) + g*l*n + h*j*o - f*l*o - g*j*p + f*k*p)
    + r*(d*k*n - c*l*n - d*j*o + b*l*o + c*j*p - b*k*p)
    + s*(-(d*g*n) + c*h*n + d*f*o - b*h*o - c*f*p + b*g*p)
    + t*(d*g*j - c*h*j - d*f*k + b*h*k + c*f*l - b*g*l);
  *a2dd = q*(h*k*m - g*l*m - h*i*o + e*l*o + g*i*p - e*k*p)
    + r*(-(d*k*m) + c*l*m + d*i*o - a*l*o - c*i*p + a*k*p)
    + s*(d*g*m - c*h*m - d*e*o + a*h*o + c*e*p - a*g*p)
    + t*(-(d*g*i) + c*h*i + d*e*k - a*h*k - c*e*l + a*g*l);
  *a3dd = q*(-(h*j*m) + f*l*m + h*i*n - e*l*n - f*i*p + e*j*p)
    + r*(d*j*m - b*l*m - d*i*n + a*l*n + b*i*p - a*j*p)
    + s*(-(d*f*m) + b*h*m + d*e*n - a*h*n - b*e*p + a*f*p)
    + t*(d*f*i - b*h*i - d*e*j + a*h*j + b*e*l - a*f*l);
  *a4dd = q*(g*j*m - f*k*m - g*i*n + e*k*n + f*i*o - e*j*o)
    + r*(-(c*j*m) + b*k*m + c*i*n - a*k*n - b*i*o + a*j*o)
    + s*(c*f*m - b*g*m - c*e*n + a*g*n + b*e*o - a*f*o)
    + t*(-(c*f*i) + b*g*i + c*e*j - a*g*j - b*e*k + a*f*k);
  *a1dd = *a1dd/determinant;
  *a2dd = *a2dd/determinant;
  *a3dd = *a3dd/determinant;
  *a4dd = *a4dd/determinant;
}

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

main()
{
  double a1, a2, a3, a4;
  double a1d, a2d, a3d, a4d;
  double t1, t2, t3, t4;
  double a1dd, a2dd, a3dd, a4dd;
  double a1ddx, a2ddx, a3ddx, a4ddx;
  double error;
  int i;

  init_dynamics();

  a1 = randomf( -3.0, 3.0 );
  a2 = randomf( -3.0, 3.0 );
  a2 = randomf( -3.0, 3.0 );
  a4 = randomf( -3.0, 3.0 );
  a1d = randomf( -20.0, 20.0 );
  a2d = randomf( -20.0, 20.0 );
  a3d = randomf( -20.0, 20.0 );
  a4d = randomf( -20.0, 20.0 );
  t1 = randomf( -5.0, 5.0 );
  t2 = randomf( -5.0, 5.0 );
  t3 = randomf( -5.0, 5.0 );
  t4 = randomf( -5.0, 5.0 );

  for( ; ; )
    {
      a1 = randomd( -3.0, 3.0 );
      a2 = randomd( -3.0, 3.0 );
      a2 = randomd( -3.0, 3.0 );
      a4 = randomd( -3.0, 3.0 );
      a1d = randomd( -20.0, 20.0 );
      a2d = randomd( -20.0, 20.0 );
      a3d = randomd( -20.0, 20.0 );
      a4d = randomd( -20.0, 20.0 );
      t1 = randomd( -5.0, 5.0 );
      t2 = randomd( -5.0, 5.0 );
      t3 = randomd( -5.0, 5.0 );
      t4 = randomd( -5.0, 5.0 );
      dynamics( a1, a2, a3, a4, a1d, a2d, a3d, a4d, 
		t1, t2, t3, t4, &a1dd, &a2dd, &a3dd, &a4dd );
      forward_dynamics( a1, a2, a3, a4, a1d, a2d, a3d, a4d, 
			t1, t2, t3, t4, &a1ddx, &a2ddx, &a3ddx, &a4ddx );
      error = SQ( a1dd - a1ddx )/(SQ(a1dd)+1) + SQ( a2dd - a2ddx )/(SQ(a2dd)+1)
	+ SQ( a3dd - a3ddx )/(SQ(a3dd)+1) + SQ( a4dd - a4ddx )/(SQ(a4dd)+1);
      // float: if ( error > 3e-4 )
      if ( error > 1e-22 )
	printf( "%g: %g %g %g; %g %g %g; %g %g %g; %g %g %g\n", error,	
		a1dd, a1ddx, a1dd - a1ddx,
		a2dd, a2ddx, a2dd - a2ddx,
		a3dd, a3ddx, a3dd - a3ddx,
		a4dd, a4ddx, a4dd - a4ddx );
    }

  /* 2.15s float, 2.39s double */
  /*
  for( i = 0; i < 1000000; i++ )
    {
      dynamics( a1, a2, a3, a4, a1d, a2d, a3d, a4d, 
		t1, t2, t3, t4, &a1dd, &a2dd, &a3dd, &a4dd );
    }
  */

  /* 3.89s float, 4.05 double */
  for( i = 0; i < 1000000; i++ )
    {
      forward_dynamics( a1, a2, a3, a4, a1d, a2d, a3d, a4d, 
			t1, t2, t3, t4, &a1ddx, &a2ddx, &a3ddx, &a4ddx );
    }
}

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