C	QGEN.FOR
C
C	routines to process positions and aspects for the MAP project
C	all sky positions will be maintained as unit vectors in the J2000
C	ecliptic coordinate system but will be STORED as 16 byte values
C	(cos(beta)cos(lambda),cos(beta)sin(lambda),sin(beta),0.)
C	This is a quaternion corresponding to a 180^o rotation about
C	an axis pointing to the position
C
C	rotations will be stored as unit-norm quaternions
C
C	all routines that return a position will be declared
C	COMPLEX*16 and will have names that start with V
C
C	all routines that return a rotation will be declared
C	COMPLEX*16 and will have names that start with Q
C
C	conversion routines
C	SUBROUTINE Q2M(Q,A) converts quaternion into a rotation matrix.
C	Matrix is such that V(sky) = A*V(body) which so it is the transpose
C	of the CSC convention but matches the COBLIB convention
C	COMPLEX*16 QMAT(A) converts matrix rotation matrix A into 
C		unit-norm quaternion
C
C	position transfer routines:
C
C	COMPLEX*16 VCEL2000(R,D) gives the position corresponding to REAL*4
C		right ascension R and declination D in J2000 radians
C	REAL*4 RA2000(V) gives the J2000 right ascension in radians
C		for COMPLEX*16 V
C	REAL*4 DEC2000(V) gives the J2000 declination in radians
C
C	COMPLEX*16 VGAL(GL,GB) gives the position for REAL*4 GL,GB = l,b
C		in radians
C	REAL*4 GLAT(V) gives b in radians
C	REAL*4 GLONG(V) gives l in radians
C
C	COMPLEX*16 VECL(EL,BETA) gives the position for 
C		REAL*4 EL,BETA = lambda,beta in radians
C	REAL*4 ELAT(V) gives the J2000 ecliptic latitude in radians
C	REAL*4 ELONG(V) gives the J2000 ecliptic longitude in radians
C
C	pixel routines:
C	INTEGER*4 PIXNO(V,RES) where RES is an integer <= 15
C		PIXNO is an integer GE 0 and LT 6*4**(RES-1)
C	COMPLEX*16 VCENPIX(NPIX,RES) returns the center of pixel # NPIX
C
	COMPLEX*16 FUNCTION QGEN(A,B,C,D)
C
C	makes a quaternion out of 4 real components
C
	REAL*4 A,B,C,D
	REAL*4 R(4)
	COMPLEX*16 Q
	EQUIVALENCE (Q,R)
	R(1) = A
	R(2) = B
	R(3) = C
	R(4) = D
	QGEN = Q
	RETURN
	END

	COMPLEX*16 FUNCTION VECL(EL,BETA)
C	converts ecliptic J2000 longitude EL and latitude BETA
C	into a position
	REAL*4 EL,BETA
	COMPLEX*16 QGEN
	VECL = QGEN(COS(BETA)*COS(EL),COS(BETA)*SIN(EL),SIN(BETA),0.)
	RETURN
	END

	COMPLEX*16 FUNCTION VCEL2000(R,D)
C	converts celestial J2000 RA R and Dec D
C	into a position
	REAL*4 R,D
	REAL*4 C(3),E(3)
	COMPLEX*16 QGEN
	C(1) = COS(R)*COS(D)
	C(2) = SIN(R)*COS(D)
	C(3) = SIN(D)
	CALL CTOE(C,E)
	VCEL2000 = QGEN(E(1),E(2),E(3),0.)
	RETURN
	END

	COMPLEX*16 FUNCTION VGAL(L,B)
C	converts galactic longitude L and latitude B into a position
	REAL*4 L,B
	REAL*4 G(3),C(3),E(3)
	COMPLEX*16 QGEN
	G(1) = COS(L)*COS(B)
	G(2) = SIN(L)*COS(B)
	G(3) = SIN(B)
	CALL GTOC(G,C)
	CALL CTOE(C,E)
	VGAL = QGEN(E(1),E(2),E(3),0.)
	RETURN
	END

	REAL*4 FUNCTION ELONG(V)
C	finds the J2000 ecliptic longitude for position V
	REAL*4 V(4)
	IF (ABS(V(4)).GT.1.E-5) THEN
	  WRITE (*,*) 'V(4) too large in ELONG = ',V(4)
	  T = SQRT(1.E-20-V(4)**2)
	ENDIF
	VM = V(1)**2+V(2)**2+V(3)**2
	IF (ABS(VM-1).GT.1.E-5) THEN
	  WRITE (*,*) 'V not normalized in ELONG = ',VM
	  T = SQRT(1.E-20-(VM-1)**2)
	ENDIF
	ELONG = ATAN2(V(2),V(1))
	RETURN
	END

	REAL*4 FUNCTION ELAT(V)
C	finds the J2000 ecliptic latitude for position V
	REAL*4 V(4)
	IF (ABS(V(4)).GT.1.E-5) THEN
	  WRITE (*,*) 'V(4) too large in ELAT = ',V(4)
	  T = SQRT(1.E-20-V(4)**2)
	ENDIF
	VM = V(1)**2+V(2)**2+V(3)**2
	IF (ABS(VM-1).GT.1.E-5) THEN
	  WRITE (*,*) 'V not normalized in ELAT = ',VM
	  T = SQRT(1.E-20-(VM-1)**2)
	ENDIF
	ELAT = ATAN2(V(3),SQRT(V(1)**2+V(2)**2))
	RETURN
	END

	REAL*4 FUNCTION GLONG(V)
C	finds the galactic longitude for position V
	REAL*4 V(4)
	REAL*4 G(3),C(3)
	REAL*4 TWOPI,GL
	PARAMETER (TWOPI = 6.28318530717958)
	IF (ABS(V(4)).GT.1.E-5) THEN
	  WRITE (*,*) 'V(4) too large in GLONG = ',V(4)
	  T = SQRT(1.E-20-V(4)**2)
	ENDIF
	VM = V(1)**2+V(2)**2+V(3)**2
	IF (ABS(VM-1).GT.1.E-5) THEN
	  WRITE (*,*) 'V not normalized in GLONG = ',VM
	  T = SQRT(1.E-20-(VM-1)**2)
	ENDIF
	CALL ETOC(V,C)
	CALL CTOG(C,G)
	GL = ATAN2(G(2),G(1))
	IF (GL.LT.0.) GL = GL+TWOPI
	GLONG = GL
	RETURN
	END

	REAL*4 FUNCTION GLAT(V)
C	finds the galactic latitude for position V
	REAL*4 V(4)
	REAL*4 G(3),C(3)
	IF (ABS(V(4)).GT.1.E-5) THEN
	  WRITE (*,*) 'V(4) too large in GLAT = ',V(4)
	  T = SQRT(1.E-20-V(4)**2)
	ENDIF
	VM = V(1)**2+V(2)**2+V(3)**2
	IF (ABS(VM-1).GT.1.E-5) THEN
	  WRITE (*,*) 'V not normalized in GLAT = ',VM
	  T = SQRT(1.E-20-(VM-1)**2)
	ENDIF
	CALL ETOC(V,C)
	CALL CTOG(C,G)
	GLAT = ATAN2(G(3),SQRT(G(1)**2+G(2)**2))
	RETURN
	END

	REAL*4 FUNCTION RA2000(V)
C	finds the J2000 right ascension for position V
	REAL*4 V(4)
	REAL*4 C(3)
	REAL*4 TWOPI,R
	PARAMETER (TWOPI = 6.28318530717958)
	IF (ABS(V(4)).GT.1.E-5) THEN
	  WRITE (*,*) 'V(4) too large in RA2000 = ',V(4)
	  T = SQRT(1.E-20-V(4)**2)
	ENDIF
	VM = V(1)**2+V(2)**2+V(3)**2
	IF (ABS(VM-1).GT.1.E-5) THEN
	  WRITE (*,*) 'V not normalized in RA2000 = ',VM
	  T = SQRT(1.E-20-(VM-1)**2)
	ENDIF
	CALL ETOC(V,C)
	R = ATAN2(C(2),C(1))
	IF (R.LT.0.) R = R+TWOPI
	RA2000 = R
	RETURN
	END

	REAL*4 FUNCTION DEC2000(V)
C	finds the J2000 declination for position V
	REAL*4 V(4)
	REAL*4 C(3)
	IF (ABS(V(4)).GT.1.E-5) THEN
	  WRITE (*,*) 'V(4) too large in DEC2000 = ',V(4)
	  T = SQRT(1.E-20-V(4)**2)
	ENDIF
	VM = V(1)**2+V(2)**2+V(3)**2
	IF (ABS(VM-1).GT.1.E-5) THEN
	  WRITE (*,*) 'V not normalized in DEC2000 = ',VM
	  T = SQRT(1.E-20-(VM-1)**2)
	ENDIF
	CALL ETOC(V,C)
	DEC2000 = ATAN2(C(3),SQRT(C(1)**2+C(2)**2))
	RETURN
	END

	COMPLEX*16 FUNCTION QADD(A,B)
C
C	adds 2 quaternions
C
	REAL*4 A(4),B(4)
	REAL*4 R(4)
	COMPLEX*16 Q
	EQUIVALENCE (Q,R)
	DO I=1,4
	  R(I) = A(I)+B(I)
	ENDDO
	QADD = Q
	RETURN
	END

	COMPLEX*16 FUNCTION QSUB(A,B)
C
C	adds 2 quaternions
C
	REAL*4 A(4),B(4)
	REAL*4 R(4)
	COMPLEX*16 Q
	EQUIVALENCE (Q,R)
	DO I=1,4
	  R(I) = A(I)-B(I)
	ENDDO
	QSUB = Q
	RETURN
	END

	COMPLEX*16 FUNCTION QXR(SCALAR,Q)
C	real SCALAR times quaterion Q
	REAL*4 SCALAR
	REAL*4 Q(4)
	REAL*4 P(4)
	COMPLEX*16 QQ
	EQUIVALENCE (QQ,P)
	DO I=1,4
	  P(I) = SCALAR*Q(I)
	ENDDO
	QXR = QQ
	RETURN
	END

	COMPLEX*16 FUNCTION QMUL(P,Q)
C	PRODUCT OF TWO QUATERNIONS P*Q
	REAL*4 P(4),Q(4),R(4)
	COMPLEX*16 QQ
	EQUIVALENCE (QQ,R)
C
C	ORDER OF COMPONENTS:	i,j,k,1
C	Reminder: 1*1=1; i*i=j*j=k*k=-1
C	1*i=i;  1*j=j;  1*k=k;
C	i*j=-j*i=k;  j*k=-k*j=i; k*i=-i*k=j
C
	R(1)=P(4)*Q(1)+Q(4)*P(1)+P(2)*Q(3)-Q(2)*P(3)
	R(2)=P(4)*Q(2)+Q(4)*P(2)+P(3)*Q(1)-Q(3)*P(1)
	R(3)=P(4)*Q(3)+Q(4)*P(3)+P(1)*Q(2)-Q(1)*P(2)
	R(4)=P(4)*Q(4)-P(1)*Q(1)-P(2)*Q(2)-P(3)*Q(3)
	QMUL = QQ
	RETURN
	END

	COMPLEX*16 FUNCTION QINVERSE(P)
	REAL*4 P(4),PINV(4)
	COMPLEX*16 Q
	EQUIVALENCE (Q,PINV)
	PP=P(1)**2+P(2)**2+P(3)**2+P(4)**2
	PINV(1)=-P(1)/PP
	PINV(2)=-P(2)/PP
	PINV(3)=-P(3)/PP
	PINV(4)=P(4)/PP
	QINVERSE = Q
	RETURN
	END	

	COMPLEX*16 FUNCTION VSUN(T68)
C	returns position of the SUN for time T68 seconds since 5/24/68
C	very approximate
C	T0 is T68 for the autumnal equinox in 2000
	REAL*8 T68
	REAL*8 YEAR,T0,TH
	REAL*4 C,S
	COMPLEX*16 QGEN
	PARAMETER (YEAR = 9.954932891146D-8, T0 = 1020384.D3)
	TH = 2*YEAR*(T68-T0)
	C = COS(TH)
	S = SIN(TH)
	VSUN = QGEN(-C,-S,0.,0.)
	RETURN
	END

	COMPLEX*16 FUNCTION QSIM(T68)
	REAL*8 T68
C
C	to simulate the MAP scan pattern.  Input is seconds since 5/24/68
C	output value is a CSC style quaternion rotating to ecliptic coords
C
C	spacecraft +z axis is the spin axis
C
C	method: five rotations are used
C	Q1 is the spin, a rotation about the z axis
C	Q2 cocks the spin axis to 22.5 degrees
C	Q3 is the precession, a rotation about the new z axis
C	Q4 rotates by 90^o to put the precession axis on the +x axis
C	Q5 is the annual motion, a rotation about the ecliptic z axis
C
	COMPLEX*16 QGEN,QMUL,Q1,Q3,Q5
	REAL*4 Q2(4)/0.1950903220161,0.,0.,0.9807852804032/
	REAL*4 Q4(4)/0.,0.7071067811865,0.,0.7071067811865/
	REAL*8 SPIN, PREC, YEAR, T0, T
C  SPIN,PREC,YEAR are 0.5*times the spin,precession and annual rate in /sec
C	T0 is the T68 time for the autumnal equinox in the year 2000
	PARAMETER (SPIN = 2.42528043975D-2, PREC = 8.726646259972D-4,
	1	YEAR = 9.954932891146D-8, T0 = 1020384.D3)
	T = T68-T0
	S = SIN(SPIN*T)
	C = COS(SPIN*T)
	Q1 = QGEN(0.,0.,S,C)
	S = SIN(PREC*T)
	C = COS(PREC*T)
	Q3 = QGEN(0.,0.,S,C)
	S = SIN(YEAR*T)
	C = COS(YEAR*T)
	Q5 = QGEN(0.,0.,S,C)
	QSIM = QMUL(Q5,QMUL(Q4,QMUL(Q3,QMUL(Q2,Q1))))
	RETURN
	END

	COMPLEX*16 FUNCTION QSIMG(T68)
C	returns quaternion for transforming body coords to galactic coords
	REAL*4 QEG(4)/0.37382019,-0.33419350,-0.64479077,-0.57690382/
	REAL*8 T68	! time in seconds since 5/24/68
	COMPLEX*16 QSIM, QMUL
	QSIMG = QMUL(QEG,QSIM(T68))
	RETURN
	END

	COMPLEX*16 FUNCTION QMAP(T68)
	REAL*8 T68
C
C	to interpolate the MAP ATTITUDE.  Input is seconds since 5/24/68
C	output value is interpolated quaternion
C
	REAL*8 TMIN/1.D0/
	REAL*8 TMAX/-1.D0/
	REAL*8 T1,XX,GMT2T68
	REAL*4 X
	REAL*4 R(4,21603)
	REAL*4 W(4)
	REAL*4 OUT(4)
	COMPLEX*16 QOUT
	EQUIVALENCE (OUT,QOUT)
	INTEGER ISTAT
	LOGICAL LSTAT
	EQUIVALENCE (ISTAT,LSTAT)
	CHARACTER*14 GMT
C	check to see if we are within the bounds of the current table
	IF(TMIN.GT.T68 .OR. T68.GT.TMAX) THEN
	  CALL T68_TO_GMT(T68,GMT)
	  ISTAT = LIB$GET_LUN(LUN)
	  IF (.NOT.LSTAT) THEN
	    WRITE (*,*) 'BAD STATUS IN WRITE_PPM LIB$GET_LUN'
	    STOP
	  ENDIF
	  WRITE (*,*) 'Opening FILE ',
	1	'SYS$DATA2:[WRIGHT.MIDEX.ATT]'//GMT(1:5)//'.ATT',
	2	' on unit',LUN
	  OPEN(UNIT=LUN,STATUS='OLD',FORM='UNFORMATTED',ERR=10,
	1	FILE = 'SYS$DATA2:[WRIGHT.MIDEX.ATT]'//GMT(1:5)//'.ATT')
	  READ (LUN) R
	  CLOSE(UNIT=LUN)
	  ISTAT = LIB$FREE_LUN(LUN)
	  IF (.NOT.LSTAT) THEN
	    WRITE (*,*) 'BAD STATUS IN QMAP LIB$FREE_LUN'
	    STOP
	  ENDIF
	  GMT(6:14) = '000000000'
	  T1 = GMT2T68(GMT)
	  TMIN = T1-1
	  TMAX = T1+86401
	ENDIF
	XX = 0.25D0*(T68-T1)
	N = XX
	N = MIN(N,21599)
C	for T68 = T1+2, N=0, X=0.5, use points 1..4
	X = XX-N
	W(1) = -X*(X-1.)*(X-2.)/6.
	W(2) =  (X+1.)*(X-1.)*(X-2.)/2.
	W(3) = -(X+1.)*X*(X-2.)/2.
	W(4) =  (X+1.)*X*(X-1.)/6.
	DO I=1,4
	  OUT(I) = W(1)*R(I,N+1)+W(2)*R(I,N+2)+W(3)*R(I,N+3)+W(4)*R(I,N+4)
	ENDDO
	QMAP = QOUT
	RETURN
10	WRITE (*,*) 'SYS$DATA2:[WRIGHT.MIDEX.ATT]'//GMT(1:5)//'.ATT',
	1	' does not exist!'
	STOP
	END

	SUBROUTINE CEL2BODY(Q,N,VCEL,VBODY)
C	given aspect quaternion Q turns N positions in VCEL
C	from sky coordinates to body coordinates in VBODY
	COMPLEX*16 Q
	INTEGER N
	REAL*4 VCEL(4,N)
	REAL*4 VBODY(4,N)
	REAL*4 A(3,3)
C	convert to matrix
	CALL Q2M(Q,A)
	DO J=1,N
	  DO I=1,3
C	    V(SC) = (A^T)*V(CEL)
	    VBODY(I,J) = A(1,I)*VCEL(1,J)+A(2,I)*VCEL(2,J)+A(3,I)*VCEL(3,J)
	  ENDDO
	  VBODY(4,J) = 0
	ENDDO
	RETURN
	END

	SUBROUTINE BODY2CEL(Q,N,VBODY,VCEL)
C	given aspect quaternion Q turns N positions in VBODY
C	from SC coordinates to SKY coordinates in VCEL
	COMPLEX*16 Q
	INTEGER N
	REAL*4 VCEL(4,N)
	REAL*4 VBODY(4,N)
	REAL*4 A(3,3)
C	convert to matrix
	CALL Q2M(Q,A)
	DO J=1,N
	  DO I=1,3
C	    V(CEL) = A*V(SC)
	    VCEL(I,J) = A(I,1)*VBODY(1,J)+A(I,2)*VBODY(2,J)+A(I,3)*VBODY(3,J)
	  ENDDO
	  VCEL(4,J) = 0
	ENDDO
	RETURN
	END

	INTEGER*4 FUNCTION PIXNO(V,RES)
C	converts position into a pixel number at resolution RES
C	RES = 9 for DIRBE, 6 for DMR, 15 for SUPER
	REAL*4 V(4)
	INTEGER*4 NSUPER
	INTEGER RES
	CALL SUPER_PIXNO(V,NSUPER)
	PIXNO = NSUPER/4**(15-RES)
	RETURN
	END

	COMPLEX*16 FUNCTION VCENPIX(NSUPER,RES)
C	converts pixel number at resolution RES into the position of
C	the pixel center
	COMPLEX*16 QGEN
	INTEGER*4 NSUPER			!Input pixel number
	INTEGER RES
	REAL*4 C(3)				!Output unit vector
C
	CALL NESTED_CENPIX(NSUPER,RES,C)
	VCENPIX = QGEN(C(1),C(2),C(3),0.)
	RETURN
	END

	COMPLEX*16 FUNCTION QMAT(A)
C	convert rotation matrix into quaternion
	REAL*4 A(3,3)
	COMPLEX*16 QGEN
	REAL*4 TRACE, TPMM, TMPM, TMMP
	REAL*4 Q1,Q2,Q3,Q4
C	from M2Q routine in COBLIB
	TRACE=A(1,1)+A(2,2)+A(3,3)
	TPMM=A(1,1)-A(2,2)-A(3,3)
	TMPM=-A(1,1)+A(2,2)-A(3,3)
	TMMP=A(3,3)-A(2,2)-A(1,1)
	IF(TMMP.GE.MAX(TRACE,TPMM,TMPM)) THEN
	  Q3=.5*SQRT(1.+TMMP)
	  FOURQ3=4.*Q3
	  Q1=(A(1,3)+A(3,1))/FOURQ3
	  Q2=(A(2,3)+A(3,2))/FOURQ3
	  Q4=(A(1,2)-A(2,1))/FOURQ3
	ELSE IF(TMPM.GT.MAX(TRACE,TPMM)) THEN
	  Q2=.5*SQRT(1.+TMPM)
	  FOURQ2=4.*Q2
	  Q1=(A(1,2)+A(2,1))/FOURQ2
	  Q3=(A(3,2)+A(2,3))/FOURQ2
	  Q4=(A(3,1)-A(1,3))/FOURQ2
	ELSE IF(TRACE.GT.TPMM) THEN
	  Q4=.5*SQRT(1.+TRACE)
	  FOURQ4=4.*Q4
	  Q1=(A(2,3)-A(3,2))/FOURQ4
	  Q2=(A(3,1)-A(1,3))/FOURQ4
	  Q3=(A(1,2)-A(2,1))/FOURQ4
	ELSE
	  Q1=.5*SQRT(1.+TPMM)
	  FOURQ1=4.*Q1
	  Q2=(A(1,2)+A(2,1))/FOURQ1
	  Q3=(A(1,3)+A(3,1))/FOURQ1
	  Q4=(A(2,3)-A(3,2))/FOURQ1
	ENDIF
C
	S=SQRT(Q1*Q1+Q2*Q2+Q3*Q3+Q4*Q4)
	IF(ABS(S-1.).GT.0.00001) TYPE *,'NORM ERROR IN NEW_M2Q'
C
	QMAT = QGEN(-Q1/S,-Q2/S,-Q3/S,Q4/S)
	RETURN
	END
