'{$STAMP BS2pe}
'{$PBASIC 2.5}
' (c) 2005 Tracy Allen, eme systems, http://www.emesystems.com
' CORDIC computation of sine and cosine from angle in degrees
' accepts whole number of degrees from -359 to +32767
' The CORDIC algorithm works for inputs in quadrants 1 and 4, so
'   this program provides a wrapper to move 0 to 360 degrees to +/- 90 degrees
' This program provides three different methods for computing the vector (#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.
' Examples:
'   45 degrees, x=10000, y=10000
'   22.5 degrees, x=9239, y=3827
'   30 degrees, x=8660, y=5000
' Enable the DEBUG statement within the CORDIC loop to see inner operation.
' Internally -90 to +90 degrees is represented as -32767 to +32767 for computations 
' 45 degrees is respresented as 16384, 90 degrees as 32767, -90 as -32768
' 

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
quadrant VAR NIB

xsin VAR WORD
xcos VAR WORD



DO
  DEBUG CR,"enter angle:"
  DEBUGIN SDEC z

  z=z+360//360   ' translate angle to 0 to 360 degrees
  quadrant=z/90    ' what quadrant?  save as 0,1,2,3
  SELECT quadrant
    CASE 1,2
      z=z-180       ' rotate 90 to 270 degrees to -90 to +90 degrees
   CASE 3 
      z=z-360       ' 4th quadrant will be 0 to -90 degrees.
  ENDSELECT
       
 'x=9239
'y=-3827

  z=-zs^((ABS z*364)+(ABS z**5825))+zs  ' translate angle -90 to +90 to -32768 to +32767
  y=0                                   ' initial unit vector length one (=10000)
  x=10000                               ' along x axis
  


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


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

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

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

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

  x=xx    ' now can update the helper variable
  
  ' 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

  x = -xs ^ (ABS x ** 39793) + xs     ' multiply to compensate CORDIC gain 0.607
  y = -ys ^ (ABS y ** 39793) + ys     ' multiply to compensate CORDIC gain 0.607

  IF quadrant=1 OR quadrant=2 THEN
    x=-x
    y=-y
  ENDIF

  x = -xs ^ (ABS x + 5 / 10) + xs    ' round off
  y = -ys ^ (ABS y + 5 / 10) + ys    ' round off


  DEBUG CR,"quadrant: ", DEC1 quadrant+1
  DEBUG TAB,"sin: ", REP "-"\ys, DEC ABS y/1000,".", DEC3 ABS y
  DEBUG TAB,"cos: ", REP "-"\xs, DEC ABS x/1000,".", DEC3 ABS x

LOOP