/* Functions dealing with proper motions and positions */

#include <math.h>

#include "libxpa.h"

#define DAYS_PER_JCY 36525.

#define SQR(X) ((X)*(X))

usPos xpa_movePm(usPos posEp0, usPos pm, double foreshort, double ep0, 
	double destEp)
/* applies proper motion pm to posEp0 to bring it from ep0 to destEp 
 *
 * We follow the fortran implementation in having all angles in rad.
 * Epochs are julian day numbers, pms are in rad/julian century.
 *
 * In pm, we *differ* from the fortran implementation in that we assume
 * pm.alpha has cos(delta) applied (i.e., is pm.alpha*cos(delta)).
 */
{
	double sd=sin(posEp0.delta), cd=cos(posEp0.delta);
	double sa=sin(posEp0.alpha), ca=cos(posEp0.alpha);
	double muAbs=sqrt(SQR(pm.alpha)+SQR(pm.delta));
	double dt=(destEp-ep0)/DAYS_PER_JCY;
	double muTot=muAbs+0.5*foreshort*dt;
	double dirA, dirD;
	double sinMot, cosMot;
	vec3 newPos;

	// Don't do computations for too small PMs to escape div by zero
	if (muAbs<1e-20) {
		return posEp0;
	}
	// this is according to Mueller, 115 (4.94)
	dirA = pm.alpha/muAbs;
	dirD = pm.delta/muAbs;
	sinMot = sin(muTot*dt);
	cosMot = cos(muTot*dt);

	newPos.x = -sd*ca*dirD*sinMot - sa*dirA*sinMot + cd*ca*cosMot;
	newPos.y = -sd*sa*dirD*sinMot + ca*dirA*sinMot + cd*sa*cosMot;
	newPos.z = +cd*dirD*sinMot + sd*cosMot;

	return xpa_v3ToUsPos(newPos);
}

void xpa_movePmAndPm(usPos posEp0, usPos pm, double foreshort, double ep0, 
	double destEp, usPos *posDestEp, usPos *pmDestEp)
/* transforms both positions and proper motion given at ep0 to epoch destEp.
 *
 * All angles are in rad, epochs are julian day numbers, PMs are in 
 * rad/julian century.
 *
 * We assume pm.alpha already has cos(delta) applied (i.e. is 
 * pm.alpha*cos(delta)).
 *
 * Evil things will happen close to the poles due to cos(delta) corrections
 * going wild.
 */
{
	double sd=sin(posEp0.delta), cd=cos(posEp0.delta);
	double sa=sin(posEp0.alpha), ca=cos(posEp0.alpha);
	double muAbs=sqrt(SQR(pm.alpha)+SQR(pm.delta));
	double dt=(destEp-ep0)/DAYS_PER_JCY;
	double muTot=muAbs+0.5*foreshort*dt;
	double muDot=muTot + 0.5*foreshort*dt; /* derivative */
	double dirA, dirD;
	double sinMot, cosMot;
	vec3 newPm, dvda, dvdd;

	// Don't do computations for too small PMs to escape div by zero
	if (muAbs<1e-20) {
		*posDestEp = posEp0;
		*pmDestEp = pm;
		return;
	}
	dirA = pm.alpha/muAbs;
	dirD = pm.delta/muAbs;
	sinMot = sin(muTot*dt);
	cosMot = cos(muTot*dt);

	newPm.x = -muDot*(+sd*ca*dirD*cosMot + sa*dirA*cosMot + cd*ca*sinMot);
	newPm.y = -muDot*(+sd*sa*dirD*cosMot - ca*dirA*cosMot + cd*sa*sinMot);
	newPm.z = -muDot*(-cd*dirD*cosMot + sd*sinMot);

	*posDestEp = xpa_movePm(posEp0, pm, foreshort, ep0, destEp);
	dvda = xpa_diffAlpha(*posDestEp);
	dvdd = xpa_diffDelta(*posDestEp);
	pmDestEp->alpha = xpa_scalProd3(&newPm, &dvda)/cos(posDestEp->delta);
	pmDestEp->delta = xpa_scalProd3(&newPm, &dvdd);
}