SpeedAngle2Offset for asm6

; Calculates a horizontal and vertical speed from a speed and an angle
; input: A (speed) Y (angle, 0-31)
; output: 0,1 (X offset), 2,3 (Y offset)
SpeedAngle2Offset:
Angle = 4
Speed = 5
  sty Angle
  sta Speed

  lda CosineTable,y
  php         ; Save whether it's negative or positive
  bpl +       ; Take the absolute value
  eor #255
  clc
  adc #1
+ ldy Speed
  jsr mul8    ; Multiply the absolute value by the speed
  sty 0       ; Write the offset
  sta 1

  plp
  bpl +          ; If it was negative, make it negative again
  sec            ;Ensure carry is set
  lda #0         ;Load constant zero
  sbc 0          ;... subtract the least significant byte
  sta 0          ;... and store the result
  lda #0         ;Load constant zero again
  sbc 1          ;... subtract the most significant byte
  sta 1          ;... and store the result
+

  ldy Angle
  lda SineTable,y
  php        ; Save whether it's positive or negative
  bpl +      ; Take the absolute value
  eor #255
  clc
  adc #1
+ ldy Speed
  jsr mul8   ; Multiply the absolute value by the speed
  sty 2
  sta 3
  plp
  bpl +           ; If it was negative, make it negative again
  sec             ;Ensure carry is set
  lda #0          ;Load constant zero
  sbc 2           ;... subtract the least significant byte
  sta 2           ;... and store the result
  lda #0          ;Load constant zero again
  sbc 3           ;... subtract the most significant byte
  sta 3           ;... and store the result
+
  rts