/****************************** COMPONENT *****************************

                                POS_PROP.C

     PROPAGATION OF A VECTOR OF ASTROMETRIC PARAMETERS 'ASTROPARAM'
          AND ITS ASSOCIATED COVARIANCE MATRIX 'COVARMATRIX'
                       FROM ONE EPOCH TO ANOTHER

  The following subroutine performs the transformation of a vector of
  astrometric parameters 'AstroParam', and its associated covariance
  matrix 'CovarMatrix', from one epoch to another.

  The positional components in AstroParam are alpha and delta expressed
  in [deg], although the corresponding elements of the covariance matrix
  refer to xi and eta, expressed in [mas] relative to the fixed normal
  triad at the position.

  FORTRAN CODE WRITTEN BY:
       L Lindegren, Lund   Observatory, 15 March 1995
  TRANSLATED FROM FORTRAN INTO C BY:
       D Priou    , Meudon Observatory, 28 March 1995

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!  WARNING : IN THE FORTRAN CODE, THE INDICES OF ALL THE ARRAYS AND       !
!            MATRICES RANGE FROM 1 (!!!!!ONE!!!!!) TO 3 OR 6.             !
!            IN THE C CODE, THE INDICES OF ALL THE ARRAYS AND MATRICES    !
!            RANGE FROM 0 (!!!!!ZERO!!!!!) TO 2 OR 5.                     !
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

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

/*----------------------------------------------------------------------
                             INCLUDES
------------------------------------------------------------------------*/
#include <math.h>


/*-----------------------------------------------------------------------
               DECLARATIONS OF THE REFERENCED COMPONENTS
 ------------------------------------------------------------------------*/
#define PUBLIC extern
#undef PUBLIC

/*-----------------------------------------------------------------------
                 PUBLIC DECLARATIONS OF THE COMPONENT
 ------------------------------------------------------------------------*/
#define PUBLIC
#include "pos_prop.h"
#undef PUBLIC

/*-----------------------------------------------------------------------
                      CONTENT OF THE COMPONENT
 ------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------
                             scalar
    Computes the scalar product of two 3-dimensional vectors x and y
-------------------------------------------------------------------------*/

REALNUMBER scalar ( REALNUMBER x[3] ,
                    REALNUMBER y[3] )
{
 return ( x[0] * y[0] + x[1] * y[1] + x[2] * y[2] ) ;
}

/*-----------------------------------------------------------------------
                             pos_prop

  Propagates the 6-dimensional vector of barycentric astrometric 
  parameters and the associated covariance matrix from epoch t0 to t
  assuming uniform space motion

  This is an implementation of the equations in The Hipparcos  
  and Tycho Catalogues (ESA SP-1200), Volume 1, Section 1.5.5,
  'Epoch Transformation: Rigorous Treatment'.

  INPUT (all real variables are REALNUMBER):

  t0       = original epoch [yr] - see Note 1
  a0[0]    = right ascension at t0 [deg]
  a0[1]    = declination at t0 [deg]
  a0[2]    = parallax at t0 [mas]
  a0[3]    = proper motion in R.A., mult by cos(Dec), at t0 [mas/yr]
  a0[4]    = proper motion in Dec at t0 [mas/yr]
  a0[5]    = normalised radial velocity at t0 [mas/yr] - see Note 2
  c0[i][j] = cov(a0[i],a0[j]) in [mas^2, mas^2/yr, mas^2/yr^2]
             (i, j = 0, 1, .., 5) - see Note 3
  t        = new epoch [yr] - see Note 1


  OUTPUT (all real variables are REALNUMBER):

  a[0]    = right ascension at t [deg]
  a[1]    = declination at t [deg]
  a[2]    = parallax at t [mas]
  a[3]    = proper motion in R.A., mult by cos(Dec), at t [mas/yr]
  a[4]    = proper motion in Dec at t [mas/yr]
  a[5]    = normalised radial velocity at t [mas/yr]
  c[i][j] = cov(a[i],a[j]) in [mas^2, mas^2/yr, mas^2/yr^2]
            (i, j = 0, 1, .., 5) - see Note 3

  FUNCTIONS/SUBROUTINES CALLED:

  REALNUMBER FUNCTION scalar = scalar product of two vectors

  NOTES:

  1. Only t-t0 is used; the origin of the time scale is
     therefore irrelevant but must be the same for t0 and t.
  
  2. The normalised radial velocity at epoch t0 is given by
        a0[5] = vr0*a0[2]/4.740470446
     where vr0 is the barycentric radial velocity in [km/s] at
     epoch t0; similarly a[5] = vr*a[2]/4.740470446 at epoch t.
  
  3. The matrices c0 and c are dimensioned as c0[6][6], c[6][6] 
     in this subroutine and must have compatible dimensions in the
     calling program unit.  Although c0 and c are symmetric 
     matrices, all elements in c0 must be defined on entry, and 
     all elements in c are defined on return.
  
  4. This routine adheres to ANSI C.

------------------------------------------------------------------------*/

void pos_prop ( REALNUMBER t0       ,
                REALNUMBER a0[6]    ,
                REALNUMBER c0[6][6] ,
                REALNUMBER t        ,
                REALNUMBER a[6]     ,
                REALNUMBER c[6][6]  )
{
 /* Constants */
#define ZERO   0.0e0
#define ONE    1.0e0
#define TWO    2.0e0
#define THREE  3.0e0
#define PI     3.14159265358979324e0
#define RDPDEG (3.14159265358979324e0/180.0e0)
#define RDPMAS (3.14159265358979324e0/180.0e0/3600.0e3)
#define EPS    1.0e-9

 /* Auxiliary variables */
 int         i, j, k, l ;
 REALNUMBER  tau, alpha0, delta0, par0, pma0, pmd0, zeta0 ;
 REALNUMBER  ca0, sa0, cd0, sd0, xy, alpha, delta, par, pma, pmd, zeta ;
 REALNUMBER  w, f, f2, f3, f4, tau2, pm02, pp0, pq0, pr0, qp0, qq0, qr0 ;
 REALNUMBER  ppmz, qpmz, sum, r0[3], p0[3], q0[3], pmv0[3], d[6][6] ;
 REALNUMBER  pmv[3], p[3], q[3], r[3], pmz[3] ;

 /* Convert input data to internal units (rad, year) */
 tau    = t - t0         ;
 alpha0 = a0[0] * RDPDEG ;
 delta0 = a0[1] * RDPDEG ;
 par0   = a0[2] * RDPMAS ;
 pma0   = a0[3] * RDPMAS ;
 pmd0   = a0[4] * RDPMAS ;
 zeta0  = a0[5] * RDPMAS ;

 /* Calculate normal triad [p0 q0 r0] at t0; r0 is
    also the unit vector to the star at epoch t0 */
 ca0 = COSINE ( alpha0 ) ;
 sa0 = SINE ( alpha0 ) ;
 cd0 = COSINE ( delta0 ) ;
 sd0 = SINE ( delta0 ) ;

 p0[0] = - sa0  ;
 p0[1] =   ca0  ;
 p0[2] =   ZERO ;

 q0[0] = - sd0 * ca0 ;
 q0[1] = - sd0 * sa0 ;
 q0[2] =   cd0       ;

 r0[0] = cd0 * ca0 ;
 r0[1] = cd0 * sa0 ;
 r0[2] = sd0       ;

 /* Proper motion vector */
 pmv0[0] = p0[0] * pma0 + q0[0] * pmd0 ;
 pmv0[1] = p0[1] * pma0 + q0[1] * pmd0 ;
 pmv0[2] = p0[2] * pma0 + q0[2] * pmd0 ;

 /* Various auxiliary quantities */
 tau2 = tau * tau ;
 pm02 = pma0 * pma0 + pmd0 * pmd0 ;
 w    = ONE + zeta0 * tau ;
 f2   = ONE / ( ONE + TWO * zeta0 * tau + (pm02 + zeta0*zeta0) * tau2 ) ;
 f    = SQRT ( f2 ) ;
 f3   = f2 * f ;
 f4   = f2 * f2 ;

 /* The position vector and parallax at t */
 r[0] = ( r0[0] * w + pmv0[0] * tau ) * f ;
 r[1] = ( r0[1] * w + pmv0[1] * tau ) * f ;
 r[2] = ( r0[2] * w + pmv0[2] * tau ) * f ;

 par = par0 * f ;

 /* The proper motion vector and normalised radial velocity at t */
 pmv[0] = ( pmv0[0] * w - r0[0] * pm02 * tau ) * f3 ;
 pmv[1] = ( pmv0[1] * w - r0[1] * pm02 * tau ) * f3 ;
 pmv[2] = ( pmv0[2] * w - r0[2] * pm02 * tau ) * f3 ;

 zeta = ( zeta0 + (pm02 + zeta0 * zeta0) * tau ) * f2 ;

 /* The normal triad [p q r] at t; if r is very
    close to the pole, select p towards RA=90 deg */
 xy = SQRT(r[0] * r[0] + r[1] * r[1]) ;
 if ( xy < EPS )
    {
     p[0] = ZERO ;
     p[1] = ONE  ;
     p[2] = ZERO ;
    }
 else
    {
     p[0] = -r[1] / xy ;
     p[1] =  r[0] / xy ;
     p[2] =  ZERO      ;
    }

 q[0] = r[1] * p[2] - r[2] * p[1] ;
 q[1] = r[2] * p[0] - r[0] * p[2] ;
 q[2] = r[0] * p[1] - r[1] * p[0] ;

 /* Convert parameters at t to external units */
 alpha = ATAN2 ( -p[0] , p[1] ) ;
 if ( alpha < ZERO )
    alpha = alpha + TWO * PI ;
 delta = ATAN2 ( r[2] , xy ) ;
 pma = scalar ( p , pmv ) ;
 pmd = scalar ( q , pmv ) ;

 a[0] = alpha / RDPDEG ;
 a[1] = delta / RDPDEG ;
 a[2] = par   / RDPMAS ;
 a[3] = pma   / RDPMAS ;
 a[4] = pmd   / RDPMAS ;
 a[5] = zeta  / RDPMAS ;

 /* Auxiliary quantities for the partial derivatives */
 pmz[0] = pmv0[0] * f - THREE * pmv[0] * w ;
 pmz[1] = pmv0[1] * f - THREE * pmv[1] * w ;
 pmz[2] = pmv0[2] * f - THREE * pmv[2] * w ;

 pp0  = scalar ( p ,  p0 ) ;
 pq0  = scalar ( p ,  q0 ) ;
 pr0  = scalar ( p ,  r0 ) ;
 qp0  = scalar ( q ,  p0 ) ;
 qq0  = scalar ( q ,  q0 ) ;
 qr0  = scalar ( q ,  r0 ) ;
 ppmz = scalar ( p , pmz ) ;
 qpmz = scalar ( q , pmz ) ;

 /* Partial derivatives */
 d[0][0] = pp0 * w * f - pr0 * pma0 * tau * f ;
 d[0][1] = pq0 * w * f - pr0 * pmd0 * tau * f ;
 d[0][2] = ZERO ;
 d[0][3] = pp0 * tau * f ;
 d[0][4] = pq0 * tau * f ;
 d[0][5] = - pma * tau2 ;

 d[1][0] = qp0 * w * f - qr0 * pma0 * tau * f ;
 d[1][1] = qq0 * w * f - qr0 * pmd0 * tau * f ;
 d[1][2] = ZERO ;
 d[1][3] = qp0 * tau * f ;
 d[1][4] = qq0 * tau * f ;
 d[1][5] = -pmd * tau2 ;

 d[2][0] = ZERO ;
 d[2][1] = ZERO ;
 d[2][2] = f ;
 d[2][3] = - par * pma0 * tau2 * f2 ;
 d[2][4] = - par * pmd0 * tau2 * f2 ;
 d[2][5] = - par * w * tau * f2 ;

 d[3][0] = - pp0 * pm02 * tau * f3 - pr0 * pma0 * w * f3 ;
 d[3][1] = - pq0 * pm02 * tau * f3 - pr0 * pmd0 * w * f3 ;
 d[3][2] = ZERO ;
 d[3][3] = pp0 * w * f3 - TWO * pr0 * pma0 * tau * f3
           - THREE * pma * pma0 * tau2 * f2 ;
 d[3][4] = pq0 * w * f3 - TWO * pr0 * pmd0 * tau * f3
           - THREE * pma * pmd0 * tau2 * f2 ;
 d[3][5] = ppmz * tau * f2 ;

 d[4][0] = - qp0 * pm02 * tau * f3 - qr0 * pma0 * w * f3 ;
 d[4][1] = - qq0 * pm02 * tau * f3 - qr0 * pmd0 * w * f3 ;
 d[4][2] = ZERO ;
 d[4][3] = qp0 * w * f3 - TWO * qr0 * pma0 * tau * f3
           - THREE * pmd * pma0 * tau2 * f2 ;
 d[4][4] = qq0 * w * f3 - TWO * qr0 * pmd0 * tau * f3
           - THREE * pmd * pmd0 * tau2 * f2 ;
 d[4][5] = qpmz * tau * f2 ;

 d[5][0] = ZERO ;
 d[5][1] = ZERO ;
 d[5][2] = ZERO ;
 d[5][3] = TWO * pma0 * w * tau * f4 ;
 d[5][4] = TWO * pmd0 * w * tau * f4 ;
 d[5][5] = ( w * w - pm02 * tau2 ) * f4 ;

 /* Propagate the covariance as c = d*c0*d' */
 for ( i = 0 ; i < 6 ; i++ )
     {
      for ( j = 0 ; j < 6 ; j++ )
          {
           sum = ZERO ;
           for ( k = 0 ; k < 6 ; k++ )
               {
                for ( l = 0 ; l < 6 ; l++ )
                    sum +=  d[i][k] * c0[k][l] * d[j][l] ;
               }
           c[i][j] = sum ;
          }
     }
}

/*-----------------------------------------------------------------------
                         END OF THE COMPONENT
 ------------------------------------------------------------------------*/

