#include <math.h>
#include "../h/robotMath.h"
#include "../h/cdefs.h"

/* need to define atan2() because vxWorks does not define it (ugh!) */

double atan2(y,x)
double y, x;
{
	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 */
	       { return (atan(y/x));
	       }
	      else			/* octant 2, take x over y */
	       { return (PIB2 - atan(x/y));
	       }
	    }
	   else
	    { if (y >= -x)		/* octant 3, take -x over y */
	       { return (PIB2 + atan(-x/y));
	       }
	      else			/* octant 4, take y over -x */
	       { return (PI - atan(-y/x));
	       }
	    }
	 }
	else
	 { if (x < 0.)
	    { if (-x >= -y)		/* octant 5, take -y over -x */
	       { return (-PI + atan(y/x));
	       }
	      else			/* octant 6, take -x over -y */
	       { return (-PIB2 - atan(x/y));
	       }
	    }
	   else
	    { if (-y >= x)		/* octant 7, take x over -y */
	       { return (-PIB2 + atan(-x/y));
	       }
	      else			/* octant 8, take -y over x */
	       { return (-atan(-y/x));
	       }
	    }
	 }
}
