/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Michael C. Wrinn.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	atan.c
 *
 * DESCRIPTION:	Contains code for the table driven ATAN2() routines.
 * 
 * AUTHOR/DATE:	John Lloyd and Mike Wrinn, Fall 1986
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>
#include "ATAN2_Cdefs.h"

#ifdef vax
asm ("#include <ATAN2_Adefs.h>");	/* asm doesn't allow nested quotes */

asm ("#define PI	0f3.14159265359");
asm ("#define PID2	0f1.57079632679");
asm ("#define NPI	0f-3.14159265359");
asm ("#define NPID2	0f-1.57079632679");
asm ("#define HUGE	0f1.7014117331926e38");
#endif

#define PI2  6.28318530718
#define PID2 1.57079632679
#define PI   3.14159265359

float ATAN2 (y, x)
double y, x;
{
#ifdef vax
	asm("	cvtdf	4(ap),r2")
	asm("	jlss	bottom")
	asm("top:")
	asm("	cvtdf	12(ap),r3")
	asm("	jlss	quadrant2")
	asm("quadrant1:")
	asm("	cmpf	r3,r2")
	asm("	blss	1f")
	asm("	clrf	r5")
	asm("	brb	tanyx")
	asm("quadrant2:")
	asm("	mnegf	r3,r0")
	asm("	cmpf	r0,r2")
	asm("	jgeq	2f")
	asm("1:")
	asm("	movf	$PID2,r5")
	asm("	brb	tanxy")
	asm("2:")
	asm("	movf	$PI,r5")
	asm("	brb	tanyx")
	asm("bottom:")
	asm("	cvtdf	12(ap),r3")
	asm("	jgeq	quadrant4")
	asm("quadrant3:")
	asm("	cmpf	r3,r2")
	asm("	jgtr	1f")
	asm("	movf	$NPI,r5")
	asm("	brb	tanyx")
	asm("quadrant4:")
	asm("	mnegf	r2,r0")
	asm("	cmpf	r0,r3")
	asm("	jlss	2f")
	asm("1:")
	asm("	movf	$NPID2,r5")
	asm("	brb	tanxy")
	asm("2:")
	asm("	clrf	r5")
	asm("	brb	tanyx")
	asm("tanyx:")
	asm("	tstf	r3")
	asm("	beql	zero")
	asm("	divf2	r3,r2")
	asm("	emodf	r2,$0,$ATAN2_TSCALE_F,r4,r0")
	asm("	movq	(_mt_atan2+ATAN2_TORIGIN_O)[r4],r2")
	asm("	mulf2	r3,r0")
	asm("	addf2	r2,r0")
	asm("	addf2	r5,r0")
	asm("	ret")
	asm("tanxy:")
	asm("	tstf	r2")
	asm("	jeql	zero")
	asm("	divf2	r2,r3")
	asm("	emodf	r3,$0,$ATAN2_TSCALE_F,r4,r0")
	asm("	movq	(_mt_atan2+ATAN2_TORIGIN_O)[r4],r2")
	asm("	mulf2	r3,r0")
	asm("	addf2	r2,r0")
	asm("	subf3	r0,r5,r0")
	asm("	ret")
	asm("zero:")
	asm("	clrf	r0")
	asm("	ret")
#else
	register struct _tabent *atp;
	register int index;
	double r;

	if (x == 0.0 && y == 0.0)
	 { return (0.0);
	 }

	/* octants are labelled 1 to 8, counter clockwise from the +X axis */

	if (y >= 0.)			/* top half */
	 { if (x >= 0.)
	    { if (x >= y)		/* octant 1, take y over x */
	       { 
		 index = (r = ATAN2_TSCALE * (y/x));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( atp->a * r + atp->b );	
	       }
	      else			/* octant 2, take x over y */
	       {
		 index = (r = ATAN2_TSCALE * (x/y));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( PID2 - (atp->a * r + atp->b) );
	       }
	    }
	   else
	    { if (y >= -x)		/* octant 3, take -x over y */
	       {
		 index = (r = ATAN2_TSCALE * (x/y));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( PID2 - (atp->a * r + atp->b) );
	       }
	      else			/* octant 4, take y over -x */
	       {
		 index = (r = ATAN2_TSCALE * (y/x));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( PI + (atp->a * r + atp->b) );
	       }
	    }
	 }
	else
	 { if (x < 0.)
	    { if (-x >= -y)		/* octant 5, take -y over -x */
	       {
		 index = (r = ATAN2_TSCALE * (y/x));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( -PI + (atp->a * r + atp->b) );
	       }
	      else			/* octant 6, take -x over -y */
	       {
		 index = (r = ATAN2_TSCALE * (x/y));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( -PID2 - (atp->a * r + atp->b) );
	       }
	    }
	   else
	    { if (-y >= x)		/* octant 7, take x over -y */
	       {
		 index = (r = ATAN2_TSCALE * (x/y));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return ( -PID2 - (atp->a * r + atp->b) );
	       }
	      else			/* octant 8, take -y over x */
	       {
		 index = (r = ATAN2_TSCALE * (y/x));
		 atp = &mt_atan2 [ index+ATAN2_TORIGIN ];
		 r -= index;
		 return (atp->a * r + atp->b);
	       }
	    }
	 }
#endif
}
