; LOAD TEST VALUES
        LD      DE,0641EH       ; HEXADECIMAL TO DE (LOWER 16-BIT)
        LD      BC,0FA61H       ; HEXADECIMAL TO BC (UPPER 16-BIT)
        CALL    SINCOS
        HALT                    ; HALT THE Z80 OR THE SIMULATOR
        
;==================================================
; SIN() COS() ROUTINE 2 * 32BIT=32BIT
; D'E'DE, B'C'BC = BCDE
; NEEDS REGISTERS ABCDEHL, CHANGES FLAGS
;
SINCOS:
        LD      (TargetAngle),DE        ; store argument
        LD      (TargetAngle+2),BC
        SUB     A                       ; A = 0
        LD      (Step),A                ; Step = 0;
        LD      L,A
        LD      H,A
        LD      (CurrAngle),HL          ; Step = 0;
        LD      (CurrAngle+2),HL
        LD      (Y),HL                  ; Y = 0;
        LD      (Y+2),HL                
        LD      HL,09DB5H               ; X = FIXED(AG_CONST); (0136E9DB5H)
        LD      (X),HL
        LD      HL,0136EH
        LD      (X+2),HL
        LD      HL,Angles               ; pAngles = Angles;                       
        LD      (pAngles),HL
                                      
SINCOSDO:                               ; do {                                                
        LD      HL,(X)                  ;   NewX = X;
        LD      (NewX),HL                                       
        LD      HL,(X+2)
        LD      (NewX+2),HL                                                                               
        LD      HL,(TargetAngle)        ;   if (TargetAngle > CurrAngle) {
        LD      DE,(CurrAngle)
        AND     A        
        SBC     HL,DE                   
        LD      HL,(TargetAngle+2)
        LD      DE,(CurrAngle+2)
        SBC     HL,DE
        JP      PE,SINCOSELSE
        LD      DE,(Y)                  ;     NewX = NewX + (Y >> Step);
        LD      BC,(Y+2)
        LD      A,(Step)
        CALL    LSRA
        LD      HL,(NewX)
        ADD     HL,DE
        LD      (NewX),HL
        LD      HL,(NewX+2)
        ADC     HL,BC
        LD      (NewX+2),HL                                                                           
        LD      DE,(X)                  ;     Y = Y - (X >> Step);
        LD      BC,(X+2)
        LD      A,(Step)
        CALL    LSRA
        LD      HL,(Y)
        AND     A
        SBC     HL,DE
        LD      (Y),HL
        LD      HL,(Y+2)
        SBC     HL,BC
        LD      (Y+2),HL
        CALL    SINCOSLD                ;     CurrAngle = CurrAngle - *pAngles++;
        LD      HL,(CurrAngle)
        AND     A
        SBC     HL,DE
        LD      (CurrAngle),HL
        LD      HL,(CurrAngle+2)
        SBC     HL,BC
        LD      (CurrAngle+2),HL
        JR      SINCOSENDIF                                  
SINCOSELSE:                             ;   } else {          
        LD      DE,(Y)                  ;     NewX = NewX - (Y >> Step);
        LD      BC,(Y+2)
        LD      A,(Step)
        CALL    LSRA
        LD      HL,(NewX)
        AND     A
        SBC     HL,DE
        LD      (NewX),HL
        LD      HL,(NewX+2)
        SBC     HL,BC
        LD      (NewX+2),HL                                        
        LD      DE,(X)                  ;     Y = Y + (X >> Step);
        LD      BC,(X+2)
        LD      A,(Step)
        CALL    LSRA
        LD      HL,(Y)
        ADD     HL,DE
        LD      (Y),HL
        LD      HL,(Y+2)
        ADC     HL,BC
        LD      (Y+2),HL                                                                             
        CALL    SINCOSLD                ;     CurrAngle = CurrAngle + *pAngles++;
        LD      HL,(CurrAngle)
        ADD     HL,DE
        LD      (CurrAngle),HL
        LD      HL,(CurrAngle+2)
        ADC     HL,BC
        LD      (CurrAngle+2),HL
SINCOSENDIF:                            ;   }                                        
        LD      HL,(NewX)               ;   X = NewX;
        LD      (X),HL                                       
        LD      HL,(NewX+2)
        LD      (X+2),HL                                        
        LD      HL,Step                 ;   ++Step;
        INC     (HL)                                                                                
        LD      A,22                    ; } while (Step <= STEPS-1);
        CP      (HL)
        JP      NC,SINCOSDO
                                        ; RESULT IS IN MEM(X) AND MEM(Y)
        LD      DE,(Y)                  
        LD      BC,(X)
        EXX
        LD      DE,(Y+2)
        LD      BC,(X+2)
        EXX
        RET                             ; RESULT IS IN D'E'DE AND B'C'BC      

;==================================================
; LOAD BCDE WITH *pAngles++
; FACTORED CODE (=WAS IDENTICAL SOURCE CODE AT SEVERAL PLACES)
;
SINCOSLD:
        LD      HL,(pAngles)
        LD      E,(HL)
        INC     HL                                        
        LD      D,(HL)
        INC     HL                                        
        LD      C,(HL)
        INC     HL                                        
        LD      B,(HL)
        INC     HL
        LD      (pAngles),HL
        RET
        
;==================================================
; N BIT SHIFT RIGHT ARITHMETRIC ROUTINE 32BIT = 32BIT
; BCDE = BCDE >> A
; CHANGES FLAGS
;
LSRA:
        SUB     8               ; SET CARRY FLAG IF A < 8
        JR      C,LSRABIT       ; NO MORE BYTES TO SHIFT
        LD      E,D	        ; SHIFT BITS 8..15 TO 7..0
        LD      D,C	        ; SHIFT BITS 16..23 TO 8..15
        LD      C,B	        ; SHIFT BITS 24..31 TO 16..23
        LD      B,0	        ; ASSUME POSITIVE NUMBER
        BIT     7,C             ; SET ZERO FLAG IF BIT 7 == 0
        JR      Z,LSRAPOS     
        DEC     B	        ; CHANGE 0 TO 0FFH
LSRAPOS:
        JR      LSRA
LSRABIT:
        ADD     A,8             ; UNDO SUB 8, SET ZERO FLAG IF A == 0
LSRALOOP:
        JR      Z,LSRAEND       ; NO MORE BITS TO SHIFT
        SRA     B
        RR      C 
        RR      D
        RR      E
        DEC     A               ; SET ZERO FLAG IF A == 0
        JR      LSRALOOP
LSRAEND:

; RESULT IS IN BCDE.
        RET

;==================================================
; variable storage (RAM)
;
X:
        DEFW    0,0
Y:     
        DEFW    0,0
TargetAngle:
        DEFW    0,0
CurrAngle:
        DEFW    0,0
NewX:
        DEFW    0,0
pAngles:
        DEFW    0
Step:
        DEFB    0
        

;==================================================
; constant storage (ROM)
;
Angles:
        DEFW    0FB54H,01921H
        DEFW    03382H,00ED6H
        DEFW    0DD7EH,007D6H
        DEFW    0B753H,003FAH
        DEFW    055BBH,001FFH
        DEFW    0EAADH,000FFH
        DEFW    0FD55H,0007FH
        DEFW    0FFAAH,0003FH
        DEFW    0FFF5H,0001FH
        DEFW    0FFFEH,0000FH
        DEFW    0FFFFH,00007H
        DEFW    0FFFFH,00003H
        DEFW    0FFFFH,00001H
        DEFW    0FFFFH,00000H
        DEFW    07FFFH,00000H
        DEFW    03FFFH,00000H
        DEFW    01FFFH,00000H
        DEFW    00FFFH,00000H
        DEFW    007FFH,00000H
        DEFW    003FFH,00000H
        DEFW    001FFH,00000H
        DEFW    000FFH,00000H
        DEFW    0007FH,00000H
        
        END