; sincos.z80
; CORDIC for sin() and cos() for the Zilog Z80 and Assembler Z80DT
; Author: Andre Adrian
; Version: 12apr2011
; length is 341 bytes

; LOAD TEST VALUES
    LD      DE,12AFH                ; Angle = -1.57079632679 (-90°)
    LD      BC,9B78H
    CALL    SINCOS
                            ; expected sin = B'C'BC = 0C0000000H (-1.0)
                            ; expected cos = D'E'DE = 000000000H (0.0)
                            ; delivered sin = B'C'BC = 0BFFFFFFFH
                            ; delivered cos = D'E'DE = 000000031H
    HALT

;==================================================
; Constants
;
CN:        EQU        22
CM:        EQU        8

;==================================================
; variable storage (RAM)
;
Angle:
    DEFW    0,0
Y:
    DEFW    0,0
X:
    DEFW    0,0
Yold:
    DEFW    0,0
pATR:
    DEFW    0
Step:
    DEFB    0

;==================================================
; constant storage (ROM)
;
ATR:
    DEFW 0F6A8H,03243H      ; atan(1)
    DEFW 06705H,01DACH      ; atan(1/2)
    DEFW 0BAFCH,00FADH      ; atan(1/4)
    DEFW 06EA6H,007F5H      ; atan(1/8)
    DEFW 0AB76H,003FEH      ; ...
    DEFW 0D55BH,001FFH
    DEFW 0FAAAH,000FFH
    DEFW 0FF55H,0007FH
    DEFW 0FFEAH,0003FH

;==================================================
; N BIT ARITHMETRIC SHIFT RIGHT ROUTINE 32BIT = 32BIT
; BCDE >>= A
; CHANGES FLAGS
;
SRBCDE:
    SUB     8               ; SET CARRY FLAG IF A < 8
    JR      C,SRBEBT        ; 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,SRBEPS
    DEC     B                   ; CHANGE + TO - SIGN
SRBEPS:
    JR      SRBCDE
SRBEBT:
    ADD     A,8             ; UNDO SUB 8, SET ZERO FLAG IF A == 0
SRBELP:
    JR      Z,SRBERT        ; NO MORE BITS TO SHIFT
    SRA     B
    RR      C
    RR      D
    RR      E
    DEC     A               ; SET ZERO FLAG IF A == 0
    JR      SRBELP
SRBERT:
    RET                                                ; RESULT IS IN BCDE.

;==================================================
; SIN() COS() ROUTINE 2 * 32BIT = 32BIT
; B'C'BC, D'E'DE = f(BCDE)
; NEEDS REGISTERS A BC DE HL, CHANGES FLAGS
;
SINCOS:
    LD      (Angle),DE      ; store argument
    LD      (Angle+2),BC
    SUB     A               ; A = 0
    LD      (Step),A        ; i = 0
    LD      L,A
    LD      H,A
    LD      (Y),HL          ; y = 0;
    LD      (Y+2),HL
    LD      HL,03B6AH       ; x = FIXED(0.607252935..);
    LD      (X),HL
    LD      HL,026DDH
    LD      (X+2),HL
    LD      HL,ATR          ; pATR = ATR;
    LD      (pATR),HL
SNCSDO:                         ; do {
    LD      HL,(Y)          ;   yold = y;
    LD      (Yold),HL
    LD      HL,(Y+2)
    LD      (Yold+2),HL
                            ;   y += (angle >= 0)? x >> i: -(x >> i);
    LD      DE,(X)          ;   x >> i
    LD      BC,(X+2)
    LD      A,(Step)
    CALL    SRBCDE
    LD      A,(Angle+3)     ;   (angle >= 0)?
    RLA                     ;   Bit 7 to Carry
    LD      HL,(Y)
    JR      C,SNCSL1
    ADD     HL,DE           ;   y += x >> i
    LD      (Y),HL
    LD      HL,(Y+2)
    ADC     HL,BC
    JR      SNCSE1
SNCSL1:
    AND     A               ;   y -= x >> i
    SBC     HL,DE
    LD      (Y),HL
    LD      HL,(Y+2)
    SBC     HL,BC
SNCSE1:
    LD      (Y+2),HL
                            ;   x -= (angle >= 0)? yold >> i: -(yold >> i);
    LD      DE,(Yold)       ;   yold >> i
    LD      BC,(Yold+2)
    LD      A,(Step)
    CALL    SRBCDE
    LD      A,(Angle+3)     ;   (angle >= 0)?
    RLA
    LD      HL,(X)
    JR      C,SNCSL2
    AND     A               ;   x -= yold >> i
    SBC     HL,DE
    LD      (X),HL
    LD      HL,(X+2)
    SBC     HL,BC
    JR      SNCSE2
SNCSL2:
    ADD     HL,DE           ;   x += yold >> i
    LD      (X),HL
    LD      HL,(X+2)
    ADC     HL,BC
SNCSE2:
    LD      (X+2),HL
                           ;   a = (i < M+1)? *pATR++: atr_fixed[M] >> (i-M);
    LD      A,(Step)       ;   (i < M+1)?
    SUB     CM+1           ;   i-(M+1)
    JR      NC,SNCSL3
    LD      HL,(pATR)      ;   *pATR++
    LD      E,(HL)
    INC     HL
    LD      D,(HL)
    INC     HL
    LD      C,(HL)
    INC     HL
    LD      B,(HL)
    INC     HL
    LD      (pATR),HL
    JR      SNCSE3
SNCSL3:
    LD      DE,(ATR+32)     ;   atr[M] >> i-M
    LD      BC,(ATR+34)
    INC     A               ;   i-M
    CALL    SRBCDE
SNCSE3:
                            ;   angle -= (angle >= 0)? a: -a;
    LD      A,(Angle+3)     ;   (angle >= 0)?
    RLA
    LD      HL,(Angle)
    JR      C,SNCSL4
    AND     A               ;   angle -= a
    SBC     HL,DE
    LD      (Angle),HL
    LD      HL,(Angle+2)
    SBC     HL,BC
    JR      SNCSE4
SNCSL4:
    ADD     HL,DE           ;   angle += a
    LD      (Angle),HL
    LD      HL,(Angle+2)
    ADC     HL,BC
SNCSE4:
    LD      (Angle+2),HL
                            ; } while (++i <= N-1);
    LD      HL,Step         ; ++i
    INC     (HL)
    LD      A,CN-1          ; N-1 >= ++i
    CP      (HL)
    JP      NC,SNCSDO
                            ; RESULT IS IN MEM(X) AND MEM(Y)
    LD      DE,(X)
    LD      BC,(Y)
    EXX
    LD      DE,(X+2)
    LD      BC,(Y+2)
    EXX
    RET                     ; RESULT IS IN D'E'DE AND B'C'BC

    END