/*
   tops.c

   Implementation for computation torque-free rotation of general rigid bodies
  
   Ramses van Zon, May 9, 2007

   References:

   [1] Lisandro Hernandez de la Pena, Ramses van Zon, Jeremy Schofield
       and Sheldon B. Opps,
       "Discontinuous molecular dynamics for semiflexible and rigid bodies"
       Journal of Chemical Physics 126, 074105 (2007).

   [2] Ramses van Zon and Jeremy Schofield,
       "Numerical implementation of the exact dynamics of free rigid bodies"
       Journal of Computational Physics, doi:10.1016/j.jcp.2006.11.019 (2007).

   [3] Ramses van Zon and Jeremy Schofield,
       "Symplectic algorithms for simulations of rigid-body systems using
       the exact solution of free motion", Physical Review E 75, 056701 (2007).
*/

#include "ctops.h"
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_precision.h>
#include <gsl/gsl_sf_ellint.h>
#include <gsl/gsl_sf_elljac.h>

/* The following macro performs an in-situ matrix multiplication: */
#define RIGHTMULTMATRIX(M1, M2, x, y)\
  x = M1.xx; y = M1.xy;\
  M1.xx *= M2.xx; M1.xx += y*M2.yx+M1.xz*M2.zx;\
  M1.xy *= M2.yy; M1.xy += x*M2.xy+M1.xz*M2.zy;\
  M1.xz *= M2.zz; M1.xz += x*M2.xz+y*M2.yz;\
  x = M1.yx; y = M1.yy;\
  M1.yx *= M2.xx; M1.yx += y*M2.yx+M1.yz*M2.zx;\
  M1.yy *= M2.yy; M1.yy += x*M2.xy+M1.yz*M2.zy;\
  M1.yz *= M2.zz; M1.yz += x*M2.xz+y*M2.yz;\
  x = M1.zx; y = M1.zy;\
  M1.zx *= M2.xx; M1.zx += y*M2.yx+M1.zz*M2.zx;\
  M1.zy *= M2.yy; M1.zy += x*M2.xy+M1.zz*M2.zy;\
  M1.zz *= M2.zz; M1.zz += x*M2.xz+y*M2.yz;

#define ODD(a) (((a)&1)==1)

/* Constants based on double precision: */
#define MACHINEPRECISION 3.E-16
#define MY_PI_2          1.57079632679489661923
#define maxNTerms        18 
#define sncndn (void)gsl_sf_elljac_e

/* Elliptic integral */
static double ellfxm(double x, double m)
{
  return x*gsl_sf_ellint_RF(1.-x*x, 1.-m*x*x, 1., GSL_PREC_DOUBLE);
}

/* Complete elliptic integral */
static double complete_ellfxm(double m)
{
  return gsl_sf_ellint_RF(0, 1.-m, 1., GSL_PREC_DOUBLE);
}

/* Square a double */
static double sqr( double x )
{
  return x*x;
}

/* Norm of a vector */
static double nrm(struct Vector * v)
{
  double biggest, absval;

  biggest = fabs(v->x);
  absval  = fabs(v->y);
  if (absval > biggest) biggest = absval;
  absval  = fabs(v->z);
  if (absval > biggest) biggest = absval;
  if (biggest != 0)
    return biggest*(sqrt(sqr(v->x/biggest)
                        +sqr(v->y/biggest)
                        +sqr(v->z/biggest)));
  else
    return 0;
}

/* * * * * * * * * * * * * * * * * * * * * *
  Rodrigues formula for a rotation matrix 
 * * * * * * * * * * * * * * * * * * * * * */
static void Rodrigues( struct Matrix * M, struct Vector * v )
{
  double theta, s, c, inrm, wx, wy, wz, oneminusc,
         wxwy1mc, wxwz1mc, wywz1mc, wxs, wys, wzs;
         
  theta = nrm(v);
  if (theta != 0) {
    s = sin(theta);
    c = cos(theta);
    inrm = 1/theta;
    wx = v->x*inrm;
    wy = v->y*inrm;
    wz = v->z*inrm;
    oneminusc = 1-c;
    wxwy1mc = wx*wy*oneminusc;
    wxwz1mc = wx*wz*oneminusc;
    wywz1mc = wy*wz*oneminusc;
    wxs = wx*s;
    wys = wy*s;
    wzs = wz*s;
    M->xx = c+wx*wx*oneminusc; M->xy = wxwy1mc-wzs;       M->xz = wxwz1mc+wys;
    M->yx = wxwy1mc+wzs;       M->yy = c+wy*wy*oneminusc; M->yz = wywz1mc-wxs;
    M->zx = wxwz1mc-wys;       M->zy = wywz1mc+wxs;       M->zz = c+wz*wz*oneminusc;
  } else {
    M->xx = 1.0; M->xy = 0.0; M->xz = 0.0;
    M->yx = 0.0; M->yy = 1.0; M->yz = 0.0;
    M->zx = 0.0; M->zy = 0.0; M->zz = 1.0;
  }
}

 
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
  Torque-free rotation of a spherical top [1,2]
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 

static void SphericalDefine( struct Top * top, double _I )
{
  top->Ix = _I;
}

static void SphericalFree( struct Top * top )
{}

static void SphericalInitialization( struct Top * top, struct Vector * omega, struct Matrix * A )
{
  top->omegab0.x = omega->x;
  top->omegab0.y = omega->y;
  top->omegab0.z = omega->z;
  top->B.xx = A->xx;
  top->B.xy = A->xy;
  top->B.xz = A->xz;
  top->B.yx = A->yx;
  top->B.yy = A->yy;
  top->B.yz = A->yz;
  top->B.zx = A->zx;
  top->B.zy = A->zy;
  top->B.zz = A->zz;
}

static void SphericalEvolution( struct Top * top, double t, struct Vector * omega, struct Matrix * A )
{
  struct Vector theta_n;
  double a, b;
  
  omega->x = top->omegab0.x;
  omega->y = top->omegab0.y;
  omega->z = top->omegab0.z;
  
  theta_n.x = -t * omega->x;
  theta_n.y = -t * omega->y;
  theta_n.z = -t * omega->z;
  
  Rodrigues( A, &theta_n );

  RIGHTMULTMATRIX( (*A), (top->B), a, b );
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  Torque-free rotation of a prolate symmetric top [1,2]
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

static void ProlateDefine( struct Top * top, double _Ix, double _Iz )
{
  top->I1 = _Iz;
  top->I3 = _Ix;
}

static void ProlateFree( struct Top * top )
{}

static void ProlateInitialization( struct Top * top, struct Vector * omega, struct Matrix * A )
{
  top->omegab0.x = omega->x;
  top->omegab0.y = omega->y;
  top->omegab0.z = omega->z;
  top->B.xx = A->xx;
  top->B.xy = A->xy;
  top->B.xz = A->xz;
  top->B.yx = A->yx;
  top->B.yy = A->yy;
  top->B.yz = A->yz;
  top->B.zx = A->zx;
  top->B.zy = A->zy;
  top->B.zz = A->zz;
  top->LboverI1.x = top->I3 * top->omegab0.x / top->I1;
  top->LboverI1.y = top->omegab0.y;
  top->LboverI1.z = top->omegab0.z;
  top->omegap = ( 1.0 - top->I3 / top->I1 ) * top->omegab0.z;
}

static void ProlateEvolution( struct Top * top, double t, struct Vector * omega, struct Matrix * A )
{
  double s, c, u, ang, a, b;
  struct Vector theta_n;
  
  ang = top->omegap * t;
  s = sin( ang );
  c = cos( ang );
  omega->z = top->omegab0.z;
  omega->y = c*top->omegab0.y + s*top->omegab0.z;
  omega->x = c*top->omegab0.z - s*top->omegab0.y;
  theta_n.x = -t * top->LboverI1.x;
  theta_n.y = -t * top->LboverI1.y;
  theta_n.z = -t * top->LboverI1.z;
  Rodrigues( A, &theta_n );
  RIGHTMULTMATRIX( (*A), (top->B), a, b );
  u = A->zz;
  A->zz *= c;
  A->zz -= A->yz*s;
  A->yz *= c;
  A->yz += u*s;
  u = A->zy;
  A->zy *= c;
  A->zy -= A->yy*s;
  A->yy *= c;
  A->yy += u*s;
  u = A->zx;
  A->zx *= c;
  A->zx -= A->yx*s;
  A->yx *= c;
  A->yx += u*s;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
   Torque-free rotation of a oblate symmetric top [1,2]
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
static void OblateDefine( struct Top * top, double _Ix, double _Iz )
{
  top->I1 = _Ix;
  top->I3 = _Iz;
}

static void OblateFree( struct Top * top )
{}

static void OblateInitialization( struct Top * top, struct Vector * omega, struct Matrix * A )
{
  top->omegab0.x = omega->x;
  top->omegab0.y = omega->y;
  top->omegab0.z = omega->z;
  top->B.xx = A->xx;
  top->B.xy = A->xy;
  top->B.xz = A->xz;
  top->B.yx = A->yx;
  top->B.yy = A->yy;
  top->B.yz = A->yz;
  top->B.zx = A->zx;
  top->B.zy = A->zy;
  top->B.zz = A->zz;
  top->LboverI1.x = top->omegab0.x;
  top->LboverI1.y = top->omegab0.y;
  top->LboverI1.z = top->I3*top->omegab0.z/top->I1;
  top->omegap = (1-top->I3/top->I1)*top->omegab0.z;
}

static void OblateEvolution( struct Top * top, double t, struct Vector * omega, struct Matrix * A )
{
  double s, c, u, ang, a, b;
  struct Vector theta_n;
  
  ang = top->omegap * t;
  s = sin(ang);
  c = cos(ang);
  omega->x = c*top->omegab0.x + s*top->omegab0.y;
  omega->y = c*top->omegab0.y - s*top->omegab0.x;
  omega->z = top->omegab0.z;

  theta_n.x = -t * top->LboverI1.x;
  theta_n.y = -t * top->LboverI1.y;
  theta_n.z = -t * top->LboverI1.z;

  Rodrigues( A, &theta_n );
  RIGHTMULTMATRIX( (*A), (top->B), a, b );
  
  u = A->xx;
  A->xx *= c;
  A->xx += A->yx*s;
  A->yx *= c;
  A->yx -= u*s;
  u = A->xy;
  A->xy *= c;
  A->xy += A->yy*s;
  A->yy *= c;
  A->yy -= u*s;
  u = A->xz;
  A->xz *= c;
  A->xz += A->yz*s;
  A->yz *= c;
  A->yz -= u*s;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  Computation of the torque-free rotation of an asymmetric top [2]
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

static void AsymmetricDefine( struct Top * top, double _Ix, double _Iy, double _Iz )
{
 top->maxNumTerms = 3;
 top->Cqr = (double*) malloc( 4*sizeof(double) );
 top->Cqi = (double*) malloc( 4*sizeof(double) );
 top->Ix = _Ix;
 top->Iy = _Iy;
 top->Iz = _Iz;
}

static void AsymmetricFree( struct Top * top )
{
  free ( top->Cqr );
  free ( top->Cqi );
}

static void AsymmetricInitialization( struct Top * top, struct Vector * omega, struct Matrix * A )
{
  double a,b,c,d,e,f,omega1,omega2,omega3,Kp,q,r,i,s,g,h;
  int n;

  a = top->Ix*omega->x;                 /* anglr momentum in x direction.*/
  b = top->Iy*omega->y;                 /* anglr momentum in y direction.*/
  c = top->Iz*omega->z;                 /* anglr momentum in z direction.*/
  d = a*a+b*b+c*c;                      /* squared norm of angular momentum.*/
  e = a*omega->x+b*omega->y+c*omega->z; /* twice the rotational energy.*/
  top->L = sqrt(d);                     /* norm of the angular momentum*/
  /* Check if Jacobi-ordering is obeyed:*/
  if ( (e>d/top->Iy && top->Ix<top->Iz) || (e<d/top->Iy && top->Ix>top->Iz) ) {
    top->orderFlag = 1;                 /* Jacobi ordering ensured by*/
    top->I1 = top->Iz;                  /* swapping the order of x and z*/
    top->I2 = top->Iy;                  /* directions and reversing the*/
    top->I3 = top->Ix;                  /* y direction.*/
    omega1 = omega->z;
    omega2 = -omega->y;
    omega3 = omega->x;
    f = hypot(b, c);   /* =Lperp, i.e. the norm of proj of L on the xy plane.*/
    /* Fill the matrix B with "T_1(0)^\dagger", using ordering:*/
    top->B.xx = -f/top->L; top->B.xy = b*a/top->L/f; top->B.xz = a*c/top->L/f;
    top->B.yx = 0;         top->B.yy = -c/f;         top->B.yz = b/f;
    top->B.zx = a/top->L;  top->B.zy = b/top->L;     top->B.zz = c/top->L;
  } else {
    top->orderFlag = 0;                /* Jacobi ordering correct!*/
    top->I1 = top->Ix;
    top->I2 = top->Iy;
    top->I3 = top->Iz;
    omega1 = omega->x;
    omega2 = omega->y;
    omega3 = omega->z;
    f = hypot(a,b);    /* =Lperp, i.e. norm of the proj of L on the xy plane*/
    /* Fill the matrix B with "T_1(0)^\dagger":*/
    top->B.xx = a*c/top->L/f; top->B.xy = b*c/top->L/f; top->B.xz = -f/top->L;
    top->B.yx = -b/f;         top->B.yy = a/f;          top->B.yz = 0;
    top->B.zx = a/top->L;     top->B.zy = b/top->L;     top->B.zz = c/top->L;
  }
  RIGHTMULTMATRIX( (top->B), (*A), g, h);/* B = T_1(0)^\dagger . A(0)*/
  a = d - e*top->I3;                     /* compute 4 subexpressions*/
  b = d - e*top->I1;
  c = top->I1 - top->I3;
  d = top->I2 - top->I3;
  top->omega1m = copysign(sqrt(a/top->I1/c),omega1);   /* amplitude of omega1(t)*/
  top->omega2m = -copysign(sqrt(omega2*omega2+top->I1*c*omega1*omega1/top->I2/d),omega1);
  top->omega3m = copysign(sqrt(-b/top->I3/c),omega3);  /* amplitude of omega3(t) */
  /* precession frequency:*/
  top->omegap = d*copysign(sqrt(b/(-d)/top->I1/top->I2/top->I3), omega3);
  top->m = a*(top->I2-top->I1)/(b*d);
  top->epsilon = ellfxm(omega2/top->omega2m, top->m);       /* `phase' in ell. fncts.*/
  top->relfreq = MY_PI_2/complete_ellfxm(top->m);      /*  freq of angular velocity*/
                                             /*   (relative to omegap)*/
  /* calculation of constants A_1, A_2 & coefficients theta fnctn series:*/
  Kp = complete_ellfxm(1-top->m);                  /* complementary quarterperiod*/
  q = exp(-2.*top->relfreq*Kp);                   /* elliptic nome.*/
  /* calculate exp{ pi/2K [K' - F(I3 omega3m/L | 1-m)]}:*/
  e = exp(top->relfreq*(copysign(Kp, omega3)-ellfxm(top->I3*top->omega3m/top->L, 1.-top->m)));
  f = e*e;                                   /* top is xi*/
  top->A2 = top->L/top->I1+top->relfreq*top->omegap*(f+1)/(f-1);      /* first term in A2 series.*/
  a = 1.;                                    /* a will be q^2n*/
  b = 1.;                                    /* b will be xi^n*/
  n = 1;
  do {
    a *= q*q;                                /* update a & b recursively.*/
    b *= f;
    c = -2.*top->relfreq*top->omegap*a/(1-a)*(b-1/b);  /* the next term in A2.*/
    top->A2 += c;                                 /* add a term of the series.*/
    n++;
  } while ( fabs(c/top->A2) > MACHINEPRECISION    /* stop upon convergence.*/
            && n < 10000 );                  /* top to ensure termination.*/
  /* determine upper bound on number of terms needed in theta fnctn series:*/
  top->numTerms = (int)(log(MACHINEPRECISION)/log(q)+.5);
  /* allocate enough memory for the coefficients in these series:*/
 if (top->numTerms > top->maxNumTerms) {
    free ( top->Cqr );
    free ( top->Cqi );
    top->Cqr = (double*) malloc( (top->numTerms+1)*sizeof(double) );
    top->Cqi = (double*) malloc( (top->numTerms+1)*sizeof(double) );
    top->maxNumTerms = top->numTerms;
  }
  a = 1.;                                      /* a will be q^2n.*/
  b = 1.;                                      /* b will be (-1)^n q^(n(n+1)).*/
  top->Cqr[0] =  (e+1/e);                      /*zeroth term in the series*/
  top->Cqi[0] = -(e-1/e);                      /*  for real and imag part*/
  s = sin(top->relfreq*top->epsilon);
  c = cos(top->relfreq*top->epsilon);
  g = 2.*c*s;                                  /*= sin(2x).*/
  h = 2.*c*c-1.;                               /*= cos(2x).*/
  r = top->Cqr[0]*s;                                /* Re(theta_1), zeroth term .*/
  i = top->Cqi[0]*c;                                /* Im(theta_1),    "     "  .*/
  for (n = 1; n <= top->numTerms; n++) {
    e *= f;                                    /* e = xi^(n+1/2).*/
    a *= q*q;                                  /* update a and b recursively*/
    b *= -a;
    top->Cqr[n] = b*(e+1/e);                        /*compute next coef Re(theta_1)*/
    top->Cqi[n] = -b*(e-1/e);                       /*compute next coef Im(theta_1)*/
    d = s;                                     /* compute sin&cos recursively.*/
    s = h*s+g*c;
    c = h*c-g*d;
    r += top->Cqr[n]*s;                             /* add next term Re(theta_1).*/
    i += top->Cqi[n]*c;                             /* add next term Im(theta_1).*/
    if ( (fabs(top->Cqr[n]) < MACHINEPRECISION)
         && (fabs(top->Cqi[n]) < MACHINEPRECISION) )
      top->numTerms = n-1;                          /*if converged, adjust numTerms*/
  }
  top->A1 = atan2(i,r);                             /* compute arg(r+iI).*/
}

static void AsymmetricEvolution( struct Top * top, double t, struct Vector * omega, struct Matrix * A )
{
  int n;
  double omega1, omega2, omega3, r, i, u, s, c, g, h, f;

  u = top->omegap*t+top->epsilon;                 /* compute argument ell fncts */
  sncndn(u, top->m, &omega2, &omega1, &omega3);  /* speedily compute sn, cn, dn */
  omega1 *= top->omega1m;                       /* multiply by the amplitudes */
  omega2 *= top->omega2m;                       /* of the respective ang. vel.*/
  omega3 *= top->omega3m;
  if (top->orderFlag == 1) {                    /* check for Jacobi ordering*/
    /* if adjusted, compensate by inverting x & z and reversing y:*/
    omega->x = omega3;
    omega->y = -omega2;
    omega->z = omega1;
    /* Below, omega1, omega2 and omega3 loose their meaning as angular
       velocities and become just dummy variables to compute T_1(t),
       taking into account the ordering*/
    omega1 *= top->I1;
    omega2 *= top->I2;
    omega3 *= top->I3/top->L;
    f = hypot(omega1, omega2);             /* This is Lperp(t)*/
    omega1 /= f;
    omega2 /= f;
    f /= top->L;
    A->xx = -f;             A->xy = 0;       A->xz = omega3;
    A->yx = -omega2*omega3; A->yy = -omega1; A->yz = -omega2*f;
    A->zx = omega1*omega3;  A->zy = -omega2; A->zz = omega1*f;
  } else {
  /* no adjustment necessary :*/
    omega->x = omega1;
    omega->y = omega2;
    omega->z = omega3;
    /* Below, omega.x, omega.y and omega.z loose their meaning as angular
     velocities and because just dummy variables to compute T_1(t)*/
    omega1 *= top->I1;
    omega2 *= top->I2;
    omega3 *= top->I3/top->L;
    f = hypot(omega1, omega2);            /* This is Lperp(t) */
    omega1 /= f;
    omega2 /= f;
    f /= top->L;
    A->xx = omega1*omega3; A->xy = -omega2; A->xz = omega1*f;
    A->yx = omega2*omega3; A->yy = omega1;  A->yz = omega2*f;
    A->zx = -f;            A->zy = 0;       A->zz = omega3;
  }
  s = sin(top->relfreq*u);
  c = cos(top->relfreq*u);
  g = 2.*c*s;                             /* g= sin(2x), h= cos(2x), used */
  h = 2.*c*c-1.;                          /* in recursion. */
  r = top->Cqr[0]*s;                           /* zeroth term Re(theta1) */
  i = top->Cqi[0]*c;                           /* zeroth term Im(theta1) */
  for (n = 1; n <= top->numTerms; n++) {       /* compute series */
    u = s;
    s = h*s+g*c;                          /* computes sin((2n+1)x) and */
    c = h*c-g*u;                          /* cos((2n+1)x) recursively. */
    r += top->Cqr[n]*s;                        /* next term in series for */
    i += top->Cqi[n]*c;                        /* r= Re(theta1) & i= Im(theta1). */
  }
  s = sin(top->A1+top->A2*t);
  c = cos(top->A1+top->A2*t);
  u = s;                                  /* use add. formula to compute */
  s = s*r-c*i;                            /* s= sin(psi) */
  c = c*r+u*i;                            /* c= sin(psi) */
  u = hypot(r,i);                         /* where psi= A1+A2 t+arg(r+iI) */
  s /= u;
  c /= u;
  /* perform mult. A =  A . T'2, where A was U* T'_1 before. */
  u = A->xx;
  A->xx *= c;
  A->xx -= A->xy*s;
  A->xy *= c;
  A->xy += u*s;
  u = A->yx;
  A->yx *= c;
  A->yx -= A->yy*s;
  A->yy *= c;
  A->yy += u*s;
  u = A->zx;
  A->zx *= c;
  A->zx -= A->zy*s;
  A->zy *= c;
  A->zy += u*s;
  /* multiply A = A->B, to give final attitide matrix: */
  RIGHTMULTMATRIX( (*A), (top->B), f, g);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    Computation of the torque-free rotation of a general asymmetric 
    top using a recursive formula for the 'last angle' [3]
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

static void RecurDefine( struct Top * top, double _Ix, double _Iy, double _Iz )
{
  double c,d,den,X[2],Y[2];
  int order,j,k,l,x,a,b;

  top->term  = (double *)  malloc( sizeof(double)  * maxNTerms );
  top->coeff = (double **) malloc( sizeof(double*) * maxNTerms );

  top->coeff[0] = (double *) malloc( sizeof(double) * 1 );
  top->coeff[0][0] = 1/2.;

  for (a = 1; a < maxNTerms; a++) {
    top->coeff[a] = (double *) malloc( sizeof(double) * (a+1) );
    top->coeff[a][0] = 1/2.;
    top->coeff[a][a] = top->coeff[a-1][a-1]/(4*a+2);
    den = 1./((2*a+2)*(2*a+1));
    for (b = 1; b < a; b++)
      top->coeff[a][b] = (2*a-b+1)*den*(top->coeff[a-1][b-1]+(2*a-b)*top->coeff[a-1][b]);
  }
  for (order=0; order<=1; order++) {
    top->q[order] = (double***)malloc(maxNTerms*sizeof(double**));
    for (j=0;j<maxNTerms;j++) {
      if (ODD(j)) {
        top->q[order][j] = (double**)malloc(j*sizeof(double*));
        for (k=0; k<j; k++)
          top->q[order][j][k] = (double*)calloc(j, sizeof(double)); /* coefficients set to zero initially*/
      } else { /* even j:*/
        top->q[order][j] = (double**)malloc((j+2)*sizeof(double*));
        for (k=0; k <= j+1; k++)
          top->q[order][j][k] = (double*)calloc(j+1, sizeof(double)); /* coefficients set to zero initially*/
      }
    }
  }

  top->Ix = _Ix;
  top->Iy = _Iy;
  top->Iz = _Iz;
  
  c = top->Iz/top->Ix - 1.;
  d = top->Iz/top->Iy - 1.;
  X[0] = c*d;       /* Note: [0] = Jacobi ordering satisfied, [1] = reversed.*/
  Y[0] = c+d;

  c = top->Ix/top->Iz - 1.;
  d = top->Ix/top->Iy - 1.;
  X[1] = c*d;
  Y[1] = c+d;

  /* set up the recursion up the maxNTerms:*/
  for (order=0; order<=1; order++)
  {
    top->q[order][0][0][0] = 1.;
    /* go to maxNTerms levels:*/
    for (j=1;j<maxNTerms;j++) {
      if (ODD(j)) {
        for (k=0; k<j; k++)
          for (l=0; l<j; l++)
            top->q[order][j][k][l] = 2*(j-k)*top->q[order][j-1][k][l];
      } else { /* even j:*/
        for (k=0; k<= j+1; k++) {
          x = 2*(j-k);
          for (l=0; l<= j; l++) {
            if (l>1 && l<=j) {
              if (k>0 && k<j)
                top->q[order][j][k][l] += top->q[order][j-1][k-1][l-2]*(x+1);
              if (k<(j-1))
                top->q[order][j][k][l] -= top->q[order][j-1][k][l-2]*x;
            }
            if (l>0 && l<j) {
              if (k>1 && k<=j)
                top->q[order][j][k][l] += Y[order]*top->q[order][j-1][k-2][l-1]*(x+2);
              if (k>0 && k<j)
                top->q[order][j][k][l] -= Y[order]*top->q[order][j-1][k-1][l-1]*(x+1);
            }
            if (l<(j-1)) {
              if (k>2 && k<(j+2))
                top->q[order][j][k][l] += X[order]*top->q[order][j-1][k-3][l]*(x+3);
              if (k>1 && k<=j)
                top->q[order][j][k][l] -= X[order]*top->q[order][j-1][k-2][l]*(x+2);
            }
          }
        }
      }
    }
  }
}

static void RecurFree( struct Top * top )
{
  int a, order, j, k;
  
  for (a=0;a<maxNTerms;a++)
    free(top->coeff[a]);
  free(top->coeff);
  free(top->term);
  for (order=0;order<=1;order++) {
    for (j=0;j<maxNTerms;j++) {
      if (ODD(j))
        for (k=0;k<j;k++)
          free(top->q[order][j][k]);
      else
        for (k=0;k<=(j+1);k++)
          free(top->q[order][j][k]);
      free(top->q[order][j]);
    }
    free(top->q[order]);
  }
}

static void RecurInitialization( struct Top * top, struct Vector * omega, struct Matrix * A )
{
  double a,b,c,d,f;
           
  a = top->Ix*omega->x,                   /* anglr momentum in x direction.*/
  b = top->Iy*omega->y,                   /* anglr momentum in y direction.*/
  c = top->Iz*omega->z,                   /* anglr momentum in z direction.*/
  d = a*a+b*b+c*c,                  /* squared norm of angular momentum.*/
  top->twiceE = a*omega->x+b*omega->y+c*omega->z; /* twice the rotational energy.*/
  top->L = sqrt(d);                     /* norm of the angular momentum*/
  /* Check if Jacobi-ordering is obeyed:*/
  if ( (top->twiceE > d/top->Iy && top->Ix < top->Iz)
        ||
       (top->twiceE < d/top->Iy && top->Ix > top->Iz) ) {
    top->orderFlag = 1;                           /* Jacobi ordering ensured by*/
    top->I1 = top->Iz;                                 /* swapping the order of x and z*/
    top->I2 = top->Iy;                                 /* directions and reversing the*/
    top->I3 = top->Ix;                                 /* y direction.*/
    top->initialomega1 = omega->z;
    top->initialomega2 = -omega->y;
    top->initialomega3 = omega->x;
    f = hypot(b, c);   /* =Lperp, i.e. the norm of proj of L on the xy plane.*/
    /* Fill the matrix B with "T_1(0)^\dagger", using ordering:*/
    a/=top->L;
    b/=f;
    c/=f;
    f/=top->L;
    top->B.xx = -f; top->B.xy = b*a; top->B.xz = a*c;
    top->B.yx = 0;  top->B.yy = -c;  top->B.yz = b;
    top->B.zx = a;  top->B.zy = b*f; top->B.zz = c*f;
  } else {
    top->orderFlag = 0;                           /* Jacobi ordering correct!*/
    top->I1 = top->Ix;
    top->I2 = top->Iy;
    top->I3 = top->Iz;
    top->initialomega1 = omega->x;
    top->initialomega2 = omega->y;
    top->initialomega3 = omega->z;
    f = hypot(a,b);    /* =Lperp, i.e. norm of the proj of L on the xy plane.*/
    /* Fill the matrix B with "T_1(0)^\dagger":*/
    a/=f;
    b/=f;
    c/=top->L;
    f/=top->L;
    top->B.xx = a*c; top->B.xy = b*c; top->B.xz = -f;
    top->B.yx = -b;  top->B.yy = a;   top->B.yz = 0;
    top->B.zx = a*f; top->B.zy = b*f; top->B.zz = c;
  }
  RIGHTMULTMATRIX( (top->B),  (*A), a, b);
  a = d - top->twiceE*top->I3;                              /* compute 4 subexpressions*/
  b = d - top->twiceE*top->I1;
  c = top->I1 - top->I3;
  d = top->I2 - top->I3;
  top->omega1m = copysign(sqrt(a/top->I1/c), top->initialomega1);   /* amplitude of omega1(t)*/
  top->omega2m = -copysign(sqrt(a/top->I2/d), top->initialomega1);  /* amplitude of omega2(t)*/
  top->omega3m = copysign(sqrt(-b/top->I3/c), top->initialomega3);  /* amplitude of omega3(t)*/
  top->omegap = d*copysign(sqrt(b/(-d)/top->I1/top->I2/top->I3), top->initialomega3);
  top->m = a*(top->I2-top->I1)/(b*d);
  top->snepsilon = top->initialomega2/top->omega2m;
  top->cnepsilon = top->initialomega1/top->omega1m;
  top->dnepsilon = top->initialomega3/top->omega3m;
}

static double invwi;
static double invwf;
static double invwi2;
static double invwf2;
static double scaledL1L2L3i;
static double scaledL1L2L3f;
static double eps;
static double LoverI3;
static double minusepsilonLoverI3nplusone;
static double epspower[maxNTerms];

static void firstOmega( struct Top * top, double omega1i, double omega2i, double omega3i,
                 double omega1f, double omega2f, double omega3f,
                 double *Omegai, double *Omegaf)
{
  double L1, L2, L3, Lsqr;
  
  L1 = top->I1*omega1i;
  L2 = top->I2*omega2i;
  L3 = top->I3*omega3i;
  Lsqr = top->L*top->L;
  eps = 1. - top->I3*top->twiceE/Lsqr;
  minusepsilonLoverI3nplusone = -eps;
  LoverI3 = top->L/top->I3;
  invwi = Lsqr/(Lsqr-L3*L3);
  invwi2 = invwi*invwi;
  scaledL1L2L3i = L1*L2*L3/Lsqr/top->L*top->I3*(top->I1-top->I2)/(top->I1*top->I2);
  L1 = top->I1*omega1f;
  L2 = top->I2*omega2f;
  L3 = top->I3*omega3f;
  invwf = Lsqr/(Lsqr-L3*L3);
  invwf2 = invwf*invwf;
  scaledL1L2L3f = L1*L2*L3/Lsqr/top->L*top->I3*(top->I1-top->I2)/(top->I1*top->I2);
  /* construct powers of eps also recursively */
  epspower[0] = 1.0;

  *Omegai = top->q[top->orderFlag][0][0][0]*invwi;
  *Omegaf = top->q[top->orderFlag][0][0][0]*invwf;

  minusepsilonLoverI3nplusone *= LoverI3;
  *Omegai *= minusepsilonLoverI3nplusone;
  *Omegaf *= minusepsilonLoverI3nplusone;

  *Omegai += LoverI3;
  *Omegaf += LoverI3;
}

static void nextOmega( struct Top * top, int j, double *Omegai, double *Omegaf)
{
  double **qj;
  double *qjk;
  double ck;
  int k, jminusone, largestl, dl, smallestl;
  
  if (j == maxNTerms) {
    /* cannot reach convergence before set number of recursions */
    *Omegai = *Omegaf = 0;  /* for lack of anything better */
  }

  epspower[j] = eps*epspower[j-1];
  *Omegai = 0.;
  *Omegaf = 0.;
  qj = top->q[top->orderFlag][j];
  if (ODD(j)) {
    for (k = 0; k < j; k++)
      {
        jminusone = j - 1;
        qjk = qj[k];
        largestl = jminusone;
        dl = k - j/2;
        if (dl > 0)
          largestl -= dl;
        ck  = qjk[largestl];
        smallestl = jminusone - k;
        while (largestl > smallestl) {
          ck *= eps;
          ck += qjk[--largestl];
        }
        ck *= epspower[smallestl];
        *Omegai = (*Omegai)*invwi+ck;
        *Omegaf = (*Omegaf)*invwf+ck;
      }
    minusepsilonLoverI3nplusone *= LoverI3;
    *Omegai *= minusepsilonLoverI3nplusone*invwi2*scaledL1L2L3i;
    *Omegaf *= minusepsilonLoverI3nplusone*invwf2*scaledL1L2L3f;
  } else {
    for (k = 0; k < (j+2); k++)
      {
        qjk = qj[k];
        largestl = j;
        dl = k - j/2;
        if (dl > 0)
          largestl -= dl;
        if (largestl < 0)
          largestl = 0;
        smallestl = j - k;
        if (smallestl < 0) smallestl = 0;
        ck = qjk[largestl];
        while (largestl > smallestl) {
          ck *= eps;
          ck += qjk[--largestl];
        }
        ck *= epspower[smallestl];
        *Omegai = (*Omegai)*invwi+ck;
        *Omegaf = (*Omegaf)*invwf+ck;
      }
    minusepsilonLoverI3nplusone *= LoverI3;
    *Omegai *= minusepsilonLoverI3nplusone;
    *Omegaf *= minusepsilonLoverI3nplusone;
  }
}

static void RecurEvolution( struct Top * top, double t, struct Vector * omega, struct Matrix * A )
{
  static double oldt;
  static const int nInitialTerms = 3;
  static double tpower[maxNTerms]; /* tpower[n] = t^(n-1) */
  
  int i, m, n, sign;
  double omega1,omega2,omega3,a,s,c,f,finalomega1,finalomega2,finalomega3,
         snwpt,cnwpt,dnwpt,snsn,cncn,dndn,den,Omegai,Omegaf,psi0,dpsi,dpsi0;

  sncndn(top->omegap*t, top->m, &snwpt, &cnwpt, &dnwpt);
  snsn = snwpt*top->snepsilon;
  cncn = cnwpt*top->cnepsilon;
  dndn = dnwpt*top->dnepsilon;
  den = 1./(1-top->m*snsn*snsn);
  omega1 = top->omega1m*(cncn-snsn*dndn)*den;
  omega2 = top->omega2m*(snwpt*top->cnepsilon*top->dnepsilon+cnwpt*dnwpt*top->snepsilon)*den;
  omega3 = top->omega3m*(dndn-top->m*snsn*cncn)*den;
  finalomega1 = omega1;
  finalomega2 = omega2;
  finalomega3 = omega3;
  if (top->orderFlag == 1) {                    /* check for Jacobi ordering */
    /* if adjusted, compensate by inverting x & z and reversing y: */
    omega->x = omega3;
    omega->y = -omega2;
    omega->z = omega1;
    /* Below, omega1, omega2 and omega3 loose their meaning as angular
       velocities and become just dummy variables to compute T_1(t),
       taking into account the ordering */
    omega1 *= top->I1;
    omega2 *= top->I2;
    omega3 *= top->I3/top->L;
    f = hypot(omega1, omega2);             /* This is Lperp(t) */
    omega1 /= f;
    omega2 /= f;
    f /= top->L;
    A->xx = -f;             A->xy = 0;       A->xz = omega3;
    A->yx = -omega2*omega3; A->yy = -omega1; A->yz = -omega2*f;
    A->zx = omega1*omega3;  A->zy = -omega2; A->zz = omega1*f;
  } else {
    /* no adjustment necessary : */
    omega->x = omega1;
    omega->y = omega2;
    omega->z = omega3;
    /* Below, omega1, omega2 and omega3 loose their meaning as angular
      velocities and because just dummy variables to compute T_1(t) */
    omega1 *= top->I1;
    omega2 *= top->I2;
    omega3 *= top->I3/top->L;
    f = hypot(omega1, omega2);            /* This is Lperp(t) */
    omega1 /= f;
    omega2 /= f;
    f /= top->L;
    A->xx = omega1*omega3; A->xy = -omega2; A->xz = omega1*f;
    A->yx = omega2*omega3; A->yy = omega1;  A->yz = omega2*f;
    A->zx = -f;            A->zy = 0;       A->zz = omega3;
  }

  if (t!=oldt) {
    oldt = t;
    tpower[0] = t;
    for (i = 1; i<maxNTerms; i++)
      tpower[i] = tpower[i-1]*t;
  }
  sign = +1;
  n = 0;
  /* first approximation: */
  firstOmega(top, top->initialomega1, top->initialomega2, top->initialomega3,
             finalomega1, finalomega2, finalomega3,
             &Omegai, &Omegaf);
  top->term[n] = tpower[n]*(Omegai + Omegaf);
  psi0 = top->coeff[0][0]*top->term[0];
  /* higher order approximations: */
  while ( n < nInitialTerms ) {
    n++;
    sign *= -1;
    nextOmega(top, n, &Omegai, &Omegaf);
    top->term[n] = tpower[n]*(Omegai+sign*Omegaf);
  }
  dpsi = 0;
  for (m = 1; m <= n; m++)
    dpsi += top->coeff[n][m]*top->term[m];
  do {
      n++;
      sign *= -1;
      nextOmega(top, n, &Omegai, &Omegaf);
      top->term[n] = tpower[n]*(Omegai+sign*Omegaf);
      dpsi0 = dpsi;
      dpsi = 0.;
      for (m = 1; m <= n; m++)
        dpsi += top->coeff[n][m]*top->term[m];
  } while ( n < (maxNTerms-1)  &&  fabs((dpsi-dpsi0)/psi0) > 1E-15 );
  s = sin(psi0 + dpsi);
  c = cos(psi0 + dpsi);
  /* perform mult. A =  A . T'2, where A was U* T'_1 before. */
  a = A->xx;
  A->xx *= c;
  A->xx -= A->xy*s;
  A->xy *= c;
  A->xy += a*s;
  a = A->yx;
  A->yx *= c;
  A->yx -= A->yy*s;
  A->yy *= c;
  A->yy += a*s;
  a = A->zx;
  A->zx *= c;
  A->zx -= A->zy*s;
  A->zy *= c;
  A->zy += a*s;

  /* multiply A = A->B, to give final attitide matrix: */
  RIGHTMULTMATRIX((*A), (top->B),s,c);
}



/*
    Implementation of the global functions
*/

/* Fill appropriate Top structure */
void DefineTop( struct Top * top, enum TopType type, double Ix, double Iy, double Iz )
{
   top->type = type;
   switch ( type ) {
     case SphericalTop:  SphericalDefine  ( top, Ix ); return;
     case OblateTop:     OblateDefine     ( top, Ix, Iz ); return;
     case ProlateTop:    ProlateDefine    ( top, Ix, Iz ); return;
     case AsymmetricTop: AsymmetricDefine ( top, Ix, Iy, Iz ); return;
     case RecurTop:      RecurDefine      ( top, Ix, Iy, Iz ); return;
  }
}

/* Initialization */
void Initialization( struct Top * top, struct Vector * omega, struct Matrix * A )
{
   switch ( top->type ) {
     case SphericalTop:  SphericalInitialization  ( top, omega, A ); return;
     case OblateTop:     OblateInitialization     ( top, omega, A ); return;
     case ProlateTop:    ProlateInitialization    ( top, omega, A ); return;
     case AsymmetricTop: AsymmetricInitialization ( top, omega, A ); return;
     case RecurTop:      RecurInitialization      ( top, omega, A ); return;
  }
}

/* To find the values of the angular momenta and attitude matrix at time t */
void Evolution( struct Top * top, double t, struct Vector * omega, struct Matrix * A )
{
   switch ( top->type ) {
     case SphericalTop:  SphericalEvolution  ( top, t, omega, A ); return;
     case OblateTop:     OblateEvolution     ( top, t, omega, A ); return;
     case ProlateTop:    ProlateEvolution    ( top, t, omega, A ); return;
     case AsymmetricTop: AsymmetricEvolution ( top, t, omega, A ); return;
     case RecurTop:      RecurEvolution      ( top, t, omega, A ); return;
  }
}
/* release memory */
void FreeTop( struct Top * top )
{
   switch ( top->type ) {
     case SphericalTop:  SphericalFree  ( top ); return;
     case OblateTop:     OblateFree     ( top ); return;
     case ProlateTop:    ProlateFree    ( top ); return;
     case AsymmetricTop: AsymmetricFree ( top ); return;
     case RecurTop:      RecurFree      ( top ); return;
  }
}
/* propagation routine */
void Propagation( struct Top * top, double dt, struct Vector * omega, struct Matrix *A )
{
  Initialization( top, omega, A );
  Evolution( top, dt, omega, A );
}