/*
   tops.cc

   Implementation of the Top classes defined in tops.h.

   Ramses van Zon, May 9, 2007

*/

#include "tops.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>

// Constants based on double precision:
#define MACHINEPRECISION 3.E-16
#define MY_PI_2          1.57079632679489661923
#define sncndn gsl_sf_elljac_e
#define maxNTerms 18
#define ODD(a) ((a)&1)

inline double ellfxm(double x, double m)
{ 
  return x*gsl_sf_ellint_RF(1.-x*x, 1.-m*x*x, 1., GSL_PREC_DOUBLE);
}
inline double complete_ellfxm(double m)
{ 
  return gsl_sf_ellint_RF(0, 1.-m, 1., GSL_PREC_DOUBLE);
}



TopSpherical::TopSpherical( double _I ) 
: I( _I ) {}

void TopSpherical::Initialization( const Vector& omega, const Matrix& A )
{
  omegab = omega;
  A0 = A;
}

void TopSpherical::Evolution( double t, Vector& omega, Matrix& A )
{
  omega = omegab;
  A = Rodrigues( (-t)*omegab ) * A0;  // Rodigues function define in vecmat3
}


TopProlate::TopProlate( double _Ix, double _Iz ) 
: I1(_Iz), I3(_Ix) {}

void TopProlate::Initialization( const Vector& omega, const Matrix& A )
{
  omegab0 = omega;
  A0 = A;
  LboverI1.x = I3 * omegab0.x / I1;
  LboverI1.y = omegab0.y;
  LboverI1.z = omegab0.z;
  omegap = ( 1-I3/I1 ) * omegab0.z;
}

void TopProlate::Evolution( double t, Vector& omega, Matrix& A )
{
  double s, c;
  SINCOS( omegap*t, &s, &c );
  omega.z = omegab0.z;
  omega.y = c*omegab0.y + s*omegab0.z;
  omega.x = c*omegab0.z - s*omegab0.y;
  A = Rodrigues( (-t)*LboverI1 ) * A0;
  double 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;
}


TopOblate::TopOblate( double _Ix, double _Iz )
: I1(_Ix), I3(_Iz)
{}

void TopOblate::Initialization( const Vector& omega, const Matrix& A )
{
  omegab0 = omega;
  A0 = A;
  LboverI1.x = omegab0.x;
  LboverI1.y = omegab0.y;
  LboverI1.z = I3*omegab0.z/I1;
  omegap = (1-I3/I1)*omegab0.z;
}

void TopOblate::Evolution( double t, Vector& omega, Matrix& A )
{
  double s, c;
  SINCOS( omegap*t, &s, &c );           // (defined in vecmat3.h)
  omega.x = c*omegab0.x + s*omegab0.y;
  omega.y = c*omegab0.y - s*omegab0.x;
  omega.z = omegab0.z;
  A = Rodrigues( (-t)*LboverI1 ) * A0;
  double 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;
}



TopAsymmetric::TopAsymmetric( double _Ix, double _Iy, double _Iz ) 
: Ix(_Ix), Iy(_Iy), Iz(_Iz), 
  maxNumTerms(3), Cqr(new DOUBLE[4]), Cqi(new DOUBLE[4])
{}

TopAsymmetric::~TopAsymmetric()
{
  delete[] Cqr;
  delete[] Cqi;
}


void TopAsymmetric::Initialization( const Vector& omega, const Matrix& A )
{
  double
    a = Ix*omega.x,                   // anglr momentum in x direction.
    b = Iy*omega.y,                   // anglr momentum in y direction.
    c = 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. 
    f,omega1,omega2,omega3,Kp,q,r,i,s,g,h;
  int n;
  L = sqrt(d);                     // norm of the angular momentum
  // Check if Jacobi-ordering is obeyed: 
  if ( (e>d/Iy && Ix<Iz) || (e<d/Iy && Ix>Iz) ) {
    orderFlag = 1;                           // Jacobi ordering ensured by    
    I1 = Iz;                                 // swapping the order of x and z 
    I2 = Iy;                                 // directions and reversing the  
    I3 = 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:            
    B.xx = -f/L; B.xy = b*a/L/f; B.xz = a*c/L/f;
    B.yx = 0;    B.yy = -c/f;    B.yz = b/f;
    B.zx = a/L;  B.zy = b/L;     B.zz = c/L;
  } else {
    orderFlag = 0;                           // Jacobi ordering correct!     
    I1 = Ix;
    I2 = Iy;
    I3 = 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": 
    B.xx = a*c/L/f; B.xy = b*c/L/f; B.xz = -f/L;
    B.yx = -b/f;    B.yy = a/f;     B.yz = 0;
    B.zx = a/L;     B.zy = b/L;     B.zz = c/L;
  }
  B *= A;                                    // B = T_1(0)^\dagger . A(0)
  a = d - e*I3;                              // compute 4 subexpressions
  b = d - e*I1;
  c = I1 - I3;
  d = I2 - I3;
  omega1m = copysign(sqrt(a/I1/c),omega1);   // amplitude of omega1(t)
  //  omega2m = -copysign(sqrt(a/I2/d),omega1);  // amplitude of omega2(t)
  omega2m = -copysign(sqrt(omega2*omega2+I1*c*omega1*omega1/I2/d),omega1);
  omega3m = copysign(sqrt(-b/I3/c),omega3);  // amplitude of omega3(t)
  // precession frequency: 
  omegap = d*copysign(sqrt(b/(-d)/I1/I2/I3), omega3);
  m = a*(I2-I1)/(b*d);

  epsilon = ellfxm(omega2/omega2m, m);       // `phase' in ell. fncts.   
  relfreq = MY_PI_2/complete_ellfxm(m);      //  freq of angular velocity
                                             //   (relative to omegap)    
  // calculation of constants A_1, A_2 & coefficients theta fnctn series:  
  Kp = complete_ellfxm(1-m);                  // complementary quarterperiod
  q = exp(-2.*relfreq*Kp);                   // elliptic nome.
  // calculate exp{ pi/2K [K' - F(I3 omega3m/L | 1-m)]}:                       
  e = exp(relfreq*(copysign(Kp, omega3)-ellfxm(I3*omega3m/L, 1.-m)));
  f = e*e;                                   // this is xi.   
  A2 = L/I1+relfreq*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.*relfreq*omegap*a/(1-a)*(b-1/b);  // the next term in A2.   
    A2 += c;                                 // add a term of the series.  
    n++;
  } while ( (fabs(c/A2) > MACHINEPRECISION)  // stop upon convergence.     
            && (n < 10000) );                // this to ensure termination.
  // determine upper bound on number of terms needed in theta fnctn series:
  numTerms = (int)(log(MACHINEPRECISION)/log(q)+.5);
  // allocate enough memory for the coefficients in these series:          
  if (numTerms > maxNumTerms) {
    delete[] Cqi;
    delete[] Cqr;
    Cqr = new double[numTerms+1];
    Cqi = new double[numTerms+1];
    maxNumTerms = numTerms;
  }
  a = 1.;                                      // a will be q^2n.
  b = 1.;                                      // b will be (-1)^n q^(n(n+1)). 
  Cqr[0] =  (e+1/e);                           //zeroth term in the series
  Cqi[0] = -(e-1/e);                           //  for real and imag part.
  SINCOS(relfreq*epsilon, &s, &c);             // gives some speed-up with g++
  g = 2.*c*s;                                  //= sin(2x).
  h = 2.*c*c-1.;                               //= cos(2x). 
  r = Cqr[0]*s;                                // Re(theta_1), zeroth term .
  i = Cqi[0]*c;                                // Im(theta_1),    "     "  .
  for (n = 1; n <= numTerms; n++) {
    e *= f;                                    // e = xi^(n+1/2). 
    a *= q*q;                                  // update a and b recursively
    b *= -a;
    Cqr[n] = b*(e+1/e);                        //compute next coef Re(theta_1)
    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 += Cqr[n]*s;                             // add next term Re(theta_1).
    i += Cqi[n]*c;                             // add next term Im(theta_1).
    if ( (fabs(Cqr[n]) < MACHINEPRECISION)
         && (fabs(Cqi[n]) < MACHINEPRECISION) )
      numTerms = n-1;                          //if converged, adjust numTerms
  }
  A1 = atan2(i,r);                             // compute arg(r+iI).
}

void TopAsymmetric::Evolution( double t, Vector& omega, Matrix& A )
{
  int n;
  double omega1, omega2, omega3;
  double r, i, u, s, c, g, h, f;

  u= omegap*t+epsilon;                     // compute argument ell fncts   
  sncndn(u, m, &omega2, &omega1, &omega3); // speedily compute sn, cn, dn  
  omega1 *= omega1m;                       // multiply by the amplitudes   
  omega2 *= omega2m;                       // of the respective ang. vel.  
  omega3 *= omega3m;
  if (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 *= I1;
    omega2 *= I2;
    omega3 *= I3/L;
    f = hypot(omega1, omega2);             // This is Lperp(t)             
    omega1 /= f;
    omega2 /= f;
    f /= 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 *= I1;
    omega2 *= I2;
    omega3 *= I3/L;
    f = hypot(omega1, omega2);            // This is Lperp(t)             
    omega1 /= f;
    omega2 /= f;
    f /= 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;
  }
  SINCOS(relfreq*u, &s, &c);              // gives speed up on gcc        
  g = 2.*c*s;                             // g= sin(2x), h= cos(2x), used
  h = 2.*c*c-1.;                          // in recursion.                
  r = Cqr[0]*s;                           // zeroth term Re(theta1)       
  i = Cqi[0]*c;                           // zeroth term Im(theta1)       
  for (n = 1; n <= 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 += Cqr[n]*s;                        // next term in series for      
    i += Cqi[n]*c;                        // r= Re(theta1) & i= Im(theta1). 
  }
  SINCOS(A1+A2*t, &s, &c);
  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:  
  A *= B;
}


TopRecur::TopRecur( double _Ix, double _Iy, double _Iz )
{
  term  = (double *)  malloc( sizeof(double)  * maxNTerms );
  coeff = (double **) malloc( sizeof(double*) * maxNTerms );

  coeff[0] = new double[1];
  coeff[0][0] = 1/2.;

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

  Ix = _Ix;
  Iy = _Iy;
  Iz = _Iz;

  double a = Iz/Ix - 1.;
  double b = Iz/Iy - 1.;
  X[0] = a*b;       // Note: [0] = Jacobi ordering satisfied, [1] = reversed.
  Y[0] = a+b;

  a = Ix/Iz - 1.;
  b = Ix/Iy - 1.;
  X[1] = a*b; 
  Y[1] = a+b;

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

TopRecur::~TopRecur()
{
  for (int a=0;a<maxNTerms;a++)
    delete[] coeff[a];
  free(coeff);
  free(term);
  for (int order=0;order<=1;order++) {
    for (int j=0;j<maxNTerms;j++) {
      if (ODD(j))
        for (int k=0;k<j;k++)
          free(q[order][j][k]);
      else
        for (int k=0;k<=(j+1);k++)
          free(q[order][j][k]);
      free(q[order][j]);
    }
    free(q[order]);
  }

}

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];

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

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

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

  Omegai += LoverI3;
  Omegaf += LoverI3;
}

// jth derivative of Omega:
inline void TopRecur::nextOmega(int j, double &Omegai, double &Omegaf)
{
  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.;
  double** qj = q[orderFlag][j];
  if (ODD(j)) { 
    for (int k = 0; k < j; k++) 
      {
	int jminusone = j - 1;
	double* qjk = qj[k];
	int largestl = jminusone;
	int dl = k - j/2;
	if (dl > 0) 
	  largestl -= dl;
	double ck  = qjk[largestl];
	int 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 (int k = 0; k < (j+2); k++) 
      {
	double* qjk = qj[k];
	int largestl = j;
	int dl = k - j/2;
	if (dl > 0)  
	  largestl -= dl;
	if (largestl < 0) 
	  largestl = 0;
	int smallestl = j - k;
	if (smallestl < 0) smallestl = 0;
	double 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;    
  }
}

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

void TopRecur::Evolution(double t, Vector& omega, Matrix& A)
{
  int n;
  double omega1;
  double omega2;
  double omega3;
  double a, s, c, f;
  double finalomega1;
  double finalomega2;
  double finalomega3;
  double snwpt, cnwpt, dnwpt;
  sncndn(omegap*t, m, &snwpt, &cnwpt, &dnwpt);
  double snsn = snwpt*snepsilon;
  double cncn = cnwpt*cnepsilon;
  double dndn = dnwpt*dnepsilon;
  double den = 1./(1-m*snsn*snsn);
  omega1 = omega1m*(cncn-snsn*dndn)*den;
  omega2 = omega2m*(snwpt*cnepsilon*dnepsilon+cnwpt*dnwpt*snepsilon)*den;
  omega3 = omega3m*(dndn-m*snsn*cncn)*den;
  finalomega1 = omega1;
  finalomega2 = omega2;
  finalomega3 = omega3;
  if (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 *= I1;
    omega2 *= I2;
    omega3 *= I3/L;
    f = hypot(omega1, omega2);             // This is Lperp(t)             
    omega1 /= f;
    omega2 /= f;
    f /= 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 *= I1;
    omega2 *= I2;
    omega3 *= I3/L;
    f = hypot(omega1, omega2);            // This is Lperp(t)             
    omega1 /= f;
    omega2 /= f;
    f /= 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;
  }
  static double oldt;
  static double tpower[maxNTerms]; // tpower[n] = t^(n-1)
  if (t!=oldt) {
    oldt = t;
    tpower[0] = t;
    for (int i = 1; i<maxNTerms; i++)
      tpower[i] = tpower[i-1]*t;
  }
  int sign = +1;
  n = 0;
  // first approximation:
  double Omegai;
  double Omegaf;
  firstOmega(initialomega1, initialomega2, initialomega3, 
	     finalomega1, finalomega2, finalomega3,
	     Omegai, Omegaf);
  term[n] = tpower[n]*(Omegai + Omegaf);
  double psi0 = coeff[0][0]*term[0];
  double dpsi, dpsi0;
  // higher order approximations:
  static const int nInitialTerms = 3;
  while ( n < nInitialTerms ) {
    n++;
    sign *= -1;
    nextOmega(n, Omegai, Omegaf);
    term[n] = tpower[n]*(Omegai+sign*Omegaf);
  }
  dpsi = 0;
  for (int k = 1; k <= n; k++) 
    dpsi += coeff[n][k]*term[k];
  do {
    n++;
    sign *= -1;
    nextOmega(n, Omegai, Omegaf);
    term[n] = tpower[n]*(Omegai+sign*Omegaf);
    dpsi0 = dpsi;
    dpsi = 0.;
    for (int k = 1; k <= n; k++) 
      dpsi += coeff[n][k]*term[k];
  } while ( (n < (maxNTerms-1)) && (fabs((dpsi-dpsi0)/psi0) > 1E-15) );
  SINCOS(psi0 + dpsi, &s, &c);

  // 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:  
  A *= B;
}