;angle is now 0 - 0xffff representing tan(angle) in the range 0 - 45 deg
;there is a lookup table for the values, 0x00, 0x40, 0x80, 0xC0 and 0x100
;the value looked up is 4 times the tan in degrees
;the values either side of the current tan are looked up
;the last 6 bits are multiplied by the difference in the lookup table
;we want the atan of the number /256
mov angle, w0
lsr w0, #14, w0 ;take most significant two bits
rcall arc_tan_table ;Get a1=atan( x>>14)
mov w1,w2 ;store in w2
inc w0, w0
rcall arc_tan_table ;get a2=atan((x>>14)+1)
sub w1, w2, w1 ;now w1 is the difference
mov #0b0011111111111111, w0
and angle, wreg
sl w0, #2, w0 ;multiply by 4 so that 0x3fff, which was nearly the next notch, has the same effect
mul.uu w0, w1, w0 ;w1 is now the less significant part multipled by the atan difference
add w1, w2, w0 ;add to the lookup value
lsr w0, #4, w0 ;divide by 16
mov w0, angle ;store as angle, 0 - 45 degrees
rcall angle_range_table
btsc w0, #9
neg angle ;negate angle if needed
bclr w0, #9
add angle ;and that should be it!
mov #360, w0
cp angle
btsc SR, #C
clr angle ;clear angle for pedants if it is still 360 or more!
bra next_bit_of_code
;angle_range is 0 - 7 here
;bit 0 is set if the angle is south
;bit 1 is set if the angle is west
;bit 2 is set if angle is closer to north/south than east/west
;The lookup table returns the angle in degrees, and with a 1 in bit 9 position if the angle needs to be reversed
angle_range_table:
mov angle_range, w0
and #7, w0
bra w0
retlw #270+512, w0
retlw #270, w0
retlw #90, w0
retlw #90+512, w0
retlw #180, w0
retlw #360+512, w0
retlw #180+512, w0
retlw #0, w0
arc_tan_table:
cp w0, #5
btss SR, #C
bra w0
retlw #12, w1
retlw #237, w1
retlw #437, w1
retlw #602, w1 ;these are 16 times the arctan of the numbers
retlw #732, w1