' {$STAMP BS2pe} 
' {$PBASIC 2.5}
' Tracy Allen, eme systems, http://www.emesystems.com
' CORDIC computation of ARCTAN from values x and y
' Computes to 2 decimal places, but accuracy is ~0.02
' Accuracy is better with larger numbers, for example atn 1/1 and atn 1000/1000
'   should both give 45 degrees, but roundoff errors affect atn 1/1 more.
' The CORDIC algorithm works for inputs in quadrants 1 and 4, so
'   this program provides a wrapper for quadrants 2 and 3.
' The anglular measure in this program assigns the value 16284 to a 45 degree angle,
'   and that is used to calculate the values in the table, tans.
' A step at the end of the program converts this to standard degrees.
' This program provides three different methods for computing the rotation (#DEFINE method=).
' One uses straihtforward IF-THEN branching on the sign, while the other
'   two eschew the IF-THEN in favor of algebraic manipulation of the sign.
' The trick for the Stamp is division of a negative number.
' Examples:
'   45 degrees, x=10000, y=10000
'   22.5 degrees, x=9239, y=3827
'   30 degrees, x=8660, y=5000
' The program also computes the length of the vector SQR(x^2+y^2),
'    again more error with small numbers.
' Enable the DEBUG statement within the CORDIC loop to see inner operation.

atans DATA Word 16384, Word 9672,Word 5110,Word 2594, Word 1302
         DATA Word 652,Word 326, Word 163, Word 81
         DATA Word 41, Word 20, Word 10, Word 5, Word 3, Word 1,Word 0
         


x VAR Word
xs VAR x.BIT15
xx VAR Word
y VAR Word
ys VAR y.BIT15
yy VAR Word
z VAR Word
zs VAR z.BIT15

atan2 VAR Word
idx VAR Nib
quadrant23 VAR Bit


DO
  DEBUG "enter x coordinate:"
  DEBUGIN SDEC x
  DEBUG "enter y coordinate:"
  DEBUGIN SDEC y
  quadrant23=xs
  IF quadrant23 THEN x=-x    ' move to 1st or 4th quadrant, where cordic works
'x=9239
'y=-3827

  z=0


  FOR idx=0 TO 15
  READ idx*2+atans, Word atan2

#DEFINE method=3  ' method 1 uses branch on sign, method 2&3 use computed direction

#SELECT method
 #CASE 1
  IF ys THEN
    ' y<0, ys=1, d=+1
      xx = x +(ABS y >> idx)  ' y<0 on entry
      y = y + (-xs ^ (ABS x >> idx) + xs)
      z = z - atan2
    ELSE
    ' y>=0, ys=0, d=-1
      xx = x + (y >> idx)      ' y>=0 on entry
      y = y - (-xs ^ (ABS x >> idx) + xs)
      z = z + atan2
  ENDIF

#CASE 2
   ' term (-ys ^ (ABS y >> idx) + ys) is division by power of 2 sign extended
   ' term -ys ^ (.) + ys is 2s complement, addition/subtraction depending on sign bit, ys.
  xx = x + (-ys ^ (-ys ^ (ABS y >> idx) + ys) + ys)
  yy = y - (-ys ^ (-xs ^ (ABS x >> idx) + xs) + ys)
  z = z + (-ys ^ atan2 + ys)
  y=yy

#CASE 3
  ' term (y >> idx | (-ys << (16-idx))) is division by power of 2 sign extended
  xx = x + (-ys ^ (y >> idx | (-ys << (16-idx))) + ys)
  yy = y - (-ys ^ (x >> idx | (-xs << (16-idx))) + ys)
  z = z + (-ys ^ atan2 + ys)
  y=yy
#ENDSELECT

  x=xx
  ' include the following DEBUG to see internal operation
  '  DEBUG CR,DEC idx,TAB,SDEC atan2,TAB,SDEC x,TAB,SDEC y,TAB,SDEC z
  NEXT

  DEBUG CR,"the angle is: ", SDEC z,TAB
  z = -zs ^ (ABS z ** 18000) + zs
  IF quadrant23 THEN                  ' wrapper for 2nd and 3rd quadrants
    IF zs THEN z=z-9000 ELSE z=z+9000
    ENDIF
  x = x ** 39793     ' vector has been rotated to x axis, multiply by CORDIC gain 0.607
  DEBUG REP "-"\zs,DEC ABS z/100, ".",DEC2 ABS z," degrees",TAB
  DEBUG "length: ",DEC x,CR,CR
  LOOP