/* @(#)ast_fk4fk5.c	19.1 (ES0-DMD) 02/25/03 13:53:53 */
/*===========================================================================
  Copyright (C) 1995 European Southern Observatory (ESO)
 
  This program is free software; you can redistribute it and/or 
  modify it under the terms of the GNU General Public License as 
  published by the Free Software Foundation; either version 2 of 
  the License, or (at your option) any later version.
 
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
 
  You should have received a copy of the GNU General Public 
  License along with this program; if not, write to the Free 
  Software Foundation, Inc., 675 Massachusetss Ave, Cambridge, 
  MA 02139, USA.
 
  Corresponding concerning ESO-MIDAS should be addressed as follows:
	Internet e-mail: midas@eso.org
	Postal address: European Southern Observatory
			Data Management Division 
			Karl-Schwarzschild-Strasse 2
			D 85748 Garching bei Muenchen 
			GERMANY
===========================================================================*/

/*+++++++++++++++       
.TYPE           Module
.IDENTIFICATION fk4fk5.c
.VERSION 1.0	05-Mar-1987: Creation .
.VERSION 1.1	26-Oct-1988: Use `s' structure
.Language       C
.AUTHOR         P.T Wallace [Starlink], Francois Ochsenbein [ESO-IPG]  
.KEYWORDS       Precession of Coordinates with proper motions,

.ENV            

.COMMENTS
This module contains routines to compute the
transformation from B1950 (FK4) to J2000 (FK5)
epochs and vice-versa. The modification concerns the position as well as the
velocity vector (radial velocity and proper motions).

\begin{TeX}

The velocity vector $\vec{v}(t)$ contains the velocity expressed in 
$''$/year as:

$$\begin{array}{ll}
  v_0 = k \pi V_{r0} 		& \\
  v_1 = \mu_{\alpha0} \cos \delta_0 &  	\\
  v_2 = \mu_{\delta0} 		&  	
\end{array}$$

The value of $k$  ($0.21094502$) when $\pi$ is
expressed in " and $V_r$ in km/s
is defined in {\tt ASTRO.H} as the mnemonic {\tt km\_s\_pc}.

The `s' structure contains the 6--vector $\{x y z \dot{x} ,\dot{y} \dot{z}\}$


This routine converts stars from the old, Bessel-Newcomb, FK4
system to the new, IAU 1976, FK5, Fricke system, using the method
of Standish (1982) as implemented by the RGO Nautical Almanac
office.  The NAO numerical values, as published on page x of the
1985 Astronomical Almanac, are used canonically.  There is
currently (April 1986) some controversy about whether this
algorithm is rigorously consistent with the FK4, the area of
doubt being the effect of differential e-terms on the proper
motions.

Note that conversion from Besselian epoch 1950.0 to
Julian epoch 2000.0 only is provided for.  Conversions
involving other epochs will require use of the appropriate
precession routines before and after this routine is called.

References:
\begin{itemize}
\item	Standish E.M., 1982: Astron. Astrophys {\bf 115}, 20.
\item	Kaplan G.H., 1981: USNO circular {\bf 163}, A2.
\item	Yokoyama K., 1983(?): {\em The guide line in the reduction of
	observations for optical astrometry during the main campaign
	in Project Merit}, International Latitude Observatory of
	Mizusawa Report.
\item	Aoki S. et al, 1983: Astron. Astrophys. {\bf 128}, 263.
\item	Astronomical Almanac, 1985, page x.
\end{itemize}

\end{TeX}

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

#include <osdefos.h>	
#include <astro.h>
#include <trigo.h>
#include <ok.h>

#define DEBUG		0	/* Debugging option	*/

RSTATIC double A[6] = {		/* For e-term	*/
	-1.62557e-6,	-0.31919e-6,	-0.13843e-6,	
	 1.244e-3,    	-1.579e-3,   	-0.660e-3    };

RSTATIC double EM[6][6] = {	/* Rotation of u + v vector	*/
  { 	 0.9999256782e0, 	-0.0111820611e0, 	-0.0048579477e0, 
		 2.42395018e-6,		-0.02710663e-6,		-0.01177656e-6},
  {	 0.0111820610e0,     	 0.9999374784e0,     	-0.0000271765e0,
		 0.02710663e-6,      	 2.42397878e-6,      	-0.00006587e-6},
  {	 0.0048579479e0,     	-0.0000271474e0,     	 0.9999881997e0,
		 0.01177656e-6,      	-0.00006582e-6,      	 2.42410173e-6},
  {	-0.000551e0,         	-0.238565e0,         	 0.435739e0,
		 0.99994704e0,       	-0.01118251e0,       	-0.00485767e0},
  {	 0.238514e0,         	-0.002667e0,         	-0.008541e0,
		 0.01118251e0,       	 0.99995883e0,       	-0.00002718e0},
  {	-0.435623e0,         	 0.012254e0,         	 0.002117e0,
		 0.00485767e0,       	-0.00002714e0,       	 1.00000956e0}
  	};

RSTATIC double EM1[6][6] = {	/* FK5 ==> FK4	*/
  {   0.9999256795e0,      0.0111814829e0,     0.0048590038e0,
	      -2.42389840e-6,      -0.02710545e-6,     -0.01177742e-6},
  {  -0.0111814828e0,      0.9999374849e0,    -0.0000271771e0,
	       0.02710545e-6,      -2.42392702e-6,      0.00006585e-6},
  {  -0.0048590040e0,     -0.0000271558e0,     0.9999881946e0,
	       0.01177742e-6,       0.00006585e-6,     -2.42404995e-6},
  {  -0.000550e0,          0.238509e0,        -0.435613e0,
  	       0.99990432e0,        0.01118145e0,       0.00485852e0},
  {  -0.238559e0,         -0.002668e0,         0.012254e0,
	      -0.01118145e0,        0.99991613e0,      -0.00002717e0},
  {   0.435730e0,         -0.008541e0,         0.002116e0,
	      -0.00485852e0,       -0.00002716e0,       0.99996684e0}
  	};


static double v1[6], v2[6];	/* 6-vectors	*/
static struct spos s;

/*============================================================================*/
int fk4_5q (qb, vb, qj, vj)
/*+++
.PURPOSE Compute J2000 (FK5) position + velocity vector from B1950 (FK4) 
	position + velocity vector.
.METHOD  See comments above.
.RETURNS OK
---*/
	double qb[2];	/* IN: Position at epoch + equinox B1950	*/
	double vb[3]; 	/* IN: Space motion in equinox	B1950	*/
	double qj[2];	/* OUT: Position at epoch + equinox J2000	*/
	double vj[3]; 	/* OUT: Space motion in equinox J2000	*/
{ 
  tr_qvs(qb, vb, &s);		/* Transform to structure	*/
  fk4_5s(&s, &s);		/* Perform change on structures	*/
  return(tr_sqv(&s, qj, vj));
}

/*============================================================================*/
int fk4_5s (sb, sj)
/*+++	
.PURPOSE Compute J2000 (FK5) unit-position + velocity vector 
	from B1950 (FK4) unit-position + velocity vector.
.METHOD  See comments above.
.RETURNS OK
---*/
	struct spos *sb; 	/* IN: Position in Phase Space B1950	*/
	struct spos *sj; 	/* OUT: Position in Phase Space J2000	*/
{ 
	double	w, dw;
	int 	i, j;

	/* Get 6-vector: In Fricke System, pm in "/century	*/

  for (i=3; i<6; i++)	v1[i] = sb->u[i] * (3600*180*100/PI);

	/* Compute scalars u.A	*/

  w 	= sb->u[0]*A[0] + sb->u[1]*A[1] + sb->u[2]*A[2];
  dw 	= sb->u[0]*A[3] + sb->u[1]*A[4] + sb->u[2]*A[5];

	/* Compute 6-vectors	*/

  for (i=0; i<3; i++)	v1[i] = sb->u[i] +  w*sb->u[i]   - A[i];
  for (i=3; i<6; i++)	v1[i] =    v1[i] + dw*sb->u[i-3] - A[i];

#if DEBUG
	printf("v1 : (%20.15f,%20.15f,%20.15f)\n",
		v1[0], v1[1], v1[2]);
	printf("   : (%20.15f,%20.15f,%20.15f)\n",
		v1[3], v1[4], v1[5]);
#endif

	/* Rotate 6-vector	*/

  for (i=0; i<6; i++)
  {	w = 0.e0;
	for (j=0; j<6; j++)	w += EM[i][j] * v1[j];
	v2[i] = w;
  }

#if DEBUG
	printf("v2 : (%20.15f,%20.15f,%20.15f)\n",
		v2[0], v2[1], v2[2]);
	printf("   : (%20.15f,%20.15f,%20.15f)\n",
		v2[3], v2[4], v2[5]);
#endif

	/* Normalize 6-vector	*/

  w = hypot(hypot(v2[0], v2[1]), v2[2]);
  for (i=0; i<6; i++)	 v2[i] /= w;

	/* Output new position + Derivative	*/

  for (i=0; i<3; i++)	sj->u[i] = v2[i];
  for (i=3; i<6; i++)	sj->u[i] = v2[i] / (3600*180*100/PI);

  return(OK);
}

/*============================================================================*/
int fk4_qv0 (qb, vb)
/*+++
.PURPOSE Compute velocity vector, assuming zero velocity in FK5 
	system.
.METHOD  Approximative
.RETURNS OK
.REMARKS Useful if proper motion not known in FK4 system.
---*/
	double qb[2];	/* IN: Position at equinox B1950	*/
	double vb[3]; 	/* OUT: Space motion in equinox	B1950	*/
{ 
	double q[2];

  tr_ou(qb, s.u);		/* Transform to structure	*/
  fk4_s0(&s);			/* Compute derivative		*/
  return(tr_sqv(&s, q, vb));
}

/*============================================================================*/
int fk4_s0 (sb)
/*+++	
.PURPOSE Compute velocity vector, assuming zero velocity in FK5 
	system.
.METHOD  Approximative
.RETURNS OK
.REMARKS Useful if proper motion not known in FK4 system.
---*/
	struct spos *sb; 	/* MOD: Position in Phase Space B1950	*/
{ 
	double	w, dw;
	int 	i, j;

#if 0		/* ============= Rigorous Method ============== */
	/* Compute scalars u.A	*/

  w 	= sb->u[0]*A[0] + sb->u[1]*A[1] + sb->u[2]*A[2];
  dw 	= sb->u[0]*A[3] + sb->u[1]*A[4] + sb->u[2]*A[5];

	/* Compute 6-vectors	*/

  for (i=0; i<3; i++)		v1[i] = sb->u[i] +  w*sb->u[i]   - A[i];
  for (i=3; i<6; i++)		v1[i] =            dw*sb->u[i-3] - A[i];

	/* Rotate 6-vector	*/

  for (i=0; i<6; i++)
  {	w = 0.e0;
	for (j=0; j<6; j++)	w += EM[i][j] * v1[j];
	v2[i] = w;
  }

	/* Normalize 6-vector, put zero as velocities in FK5	*/

  w = hypot(hypot(v2[0], v2[1]), v2[2]);
  for (i=0; i<3; i++)	 v2[i] /= w;
  for (i=3; i<6; i++)	 v2[i] = 0.e0;

	/* Rotate back 6-vector; the result IS normalized	*/

  for (i=0; i<6; i++)
  {	w = 0.e0;
	for (j=0; j<3; j++)	w += EM1[i][j] * v2[j];
	v1[i] = w;
  }

	/* Compute scalars u.A	*/

  w 	= v1[0]*A[0] + v1[1]*A[1] + v1[2]*A[2];
  dw 	= v1[0]*A[3] + v1[1]*A[4] + v1[2]*A[5];

	/* Include A--term	*/
	
  for (i=3; i<6; i++)	v1[i] = v1[i] - dw*v1[i-3] + A[i];
  for (i=0; i<3; i++)	v1[i] = v1[i] -  w*v1[i]   + A[i];
	
	/* Normalize 6-vector	*/

  w = hypot(hypot(v1[0], v1[1]), v1[2]);
  for (i=0; i<6; i++)	 v1[i] /= w;

	/* Output Derivative	*/

  for (i=3; i<6; i++)	sb->u[i] = v2[i] / (3600*180*100/PI);

#else		/* ============= Approximative Method ============== */

	/* Position in J2000 	*/

  for (i=0; i<3; i++)		v1[i] = sb->u[i];

	/* Rotate vector	*/

  for (i=0; i<3; i++)
  {	w = 0.e0;
	for (j=0; j<3; j++)	w += EM[i][j] * v1[j];
	v2[i] = w;
  }

	/* Rotate back 6-vector; the result IS normalized	*/

  for (i=0; i<6; i++)
  {	w = 0.0e0;
	for (j=0; j<3; j++)	w += EM1[i][j] * v2[j];
	v1[i] = w;
  }

	/* Compute scalars u.A	*/

  dw 	= v1[0]*A[3] + v1[1]*A[4] + v1[2]*A[5];

	/* Include A--term	*/
	
  for (i=3; i<6; i++)	v1[i] = v1[i] - dw*v1[i-3] + A[i];
	
	/* Output Derivative	*/

  for (i=3; i<6; i++)	sb->u[i] = v1[i] / (3600*180*100/PI);
#endif

  return(OK);
}

/*============================================================================*/
int fk5_4q (qj, vj, qb, vb)
/*+++	
.PURPOSE Compute B1950 (FK4) unit-position + velocity vector
	from J2000 (FK5) unit-position + velocity vector 
.METHOD  See comments above.
.RETURNS OK
---*/
	double qj[2];	/* IN: Position at epoch + equinox J2000	*/
	double vj[3]; 	/* IN: Space motion in equinox J2000	*/
	double qb[2];	/* OUT: Position at epoch + equinox B1950	*/
	double vb[3]; 	/* OUT: Space motion in equinox	B1950	*/
{ 
  tr_qvs(qj, vj, &s);		/* Transform to structure	*/
  fk4_5s(&s, &s);		/* Perform change on structures	*/
  return(tr_sqv(&s, qb, vb));
}

/*============================================================================*/
int fk5_4s (sj, sb)
/*+++	
.PURPOSE Compute B1950 (FK4) unit-position + velocity vector
	from J2000 (FK5) unit-position + velocity vector 
.METHOD  See comments above.
.RETURNS OK
---*/
	struct spos *sj; 	/* IN: Position in Phase Space J2000	*/
	struct spos *sb; 	/* OUT: Position in Phase Space B1950	*/
{ 
	double	w, dw;
	int 	i, j;

	/* Get 6-vector: In Fricke System, pm in "/century	*/

  for (i=0; i<3; i++) 	v1[i] = sj->u[i];
  for (i=3; i<6; i++)	v1[i] = sb->u[i] * (3600*180*100/PI);

	/* Rotate 6-vector	*/

  for (i=0; i<6; i++)
  {	w = 0.e0;
	for (j=0; j<6; j++)	w += EM1[i][j] * v1[j];
	v2[i] = w;
  }

	/* Normalize 6-vector	*/

  w = hypot(hypot(v2[0], v2[1]), v2[2]);
  for (i=0; i<6; i++)	 v2[i] /= w;

	/* Compute scalars u.A	*/

  w 	= v2[0]*A[0] + v2[1]*A[1] + v2[2]*A[2];
  dw 	= v2[0]*A[3] + v2[1]*A[4] + v2[2]*A[5];

	/* Include A--term	*/
	
  for (i=3; i<6; i++)	v2[i] = v2[i] - dw*v2[i-3] + A[i];
  for (i=0; i<3; i++)	v2[i] = v2[i] -  w*v2[i]   + A[i];
	
	/* Normalize 6-vector	*/

  w = hypot(hypot(v2[0], v2[1]), v2[2]);
  for (i=0; i<6; i++)	 v2[i] /= w;

	/* Output new position + Derivative	*/

  for (i=0; i<3; i++)	sb->u[i] = v2[i];
  for (i=3; i<6; i++)	sb->u[i] = v2[i] / (3600*180*100/PI);

  return(OK);
}

